added Odometry view to MainWindow, increased version to 0.7.3

CMakeLists.txt

@@ -16,7 +16,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 7)
-SET(RTABMAP_PATCH_VERSION 2)
+SET(RTABMAP_PATCH_VERSION 3)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   

corelib/src/Features2d.cpp

@@ -481,7 +481,7 @@ void SURF::parseParameters(const ParametersMap & parameters)
 		_surf = new cv::SURF(hessianThreshold_, nOctaves_, nOctaveLayers_, extended_, upright_);
 	}
 #else
-	UERROR("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
+	UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
 #endif
 }
 
@@ -502,7 +502,7 @@ std::vector<cv::KeyPoint> SURF::generateKeypointsImpl(const cv::Mat & image, con
 		_surf->detect(imgRoi, keypoints);
 	}
 #else
-	UERROR("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
+	UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
 #endif
 	return keypoints;
 }
@@ -533,7 +533,7 @@ cv::Mat SURF::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
 		_surf->compute(image, keypoints, descriptors);
 	}
 #else
-	UERROR("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
+	UWARN("RTAB-Map is not built with OpenCV nonfree module so SURF cannot be used!");
 #endif
 
 	return descriptors;
@@ -561,7 +561,7 @@ SIFT::~SIFT()
 		delete _sift;
 	}
 #else
-	UERROR("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
+	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
 #endif
 }
 
@@ -582,7 +582,7 @@ void SIFT::parseParameters(const ParametersMap & parameters)
 
 	_sift = new cv::SIFT(nfeatures_, nOctaveLayers_, contrastThreshold_, edgeThreshold_, sigma_);
 #else
-	UERROR("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
+	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
 #endif
 }
 
@@ -594,7 +594,7 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
 	cv::Mat imgRoi(image, roi);
 	_sift->detect(imgRoi, keypoints); // Opencv keypoints
 #else
-	UERROR("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
+	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
 #endif
 	return keypoints;
 }
@@ -606,7 +606,7 @@ cv::Mat SIFT::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::Key
 #if RTABMAP_NONFREE == 1
 	_sift->compute(image, keypoints, descriptors);
 #else
-	UERROR("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
+	UWARN("RTAB-Map is not built with OpenCV nonfree module so SIFT cannot be used!");
 #endif
 	return descriptors;
 }

guilib/include/rtabmap/gui/CloudViewer.h

@@ -147,7 +147,8 @@ public:
 	bool getPose(const std::string & id, Transform & pose); //including meshes
 	bool getCloudVisibility(const std::string & id);
 
-	const QMap<std::string, Transform> & getAddedClouds() {return _addedClouds;} //including meshes
+	const QMap<std::string, Transform> & getAddedClouds() const {return _addedClouds;} //including meshes
+	const QColor & getBackgroundColor() const;
 
 	void setCameraTargetLocked(bool enabled = true);
 	void setCameraTargetFollow(bool enabled = true);
@@ -197,6 +198,7 @@ private:
     std::list<std::string> _gridLines;
     QSet<Qt::Key> _keysPressed;
     QString _workingDirectory;
+    QColor _backgroundColor;
 };
 
 } /* namespace rtabmap */

guilib/src/CloudViewer.cpp

@@ -74,7 +74,8 @@ CloudViewer::CloudViewer(QWidget *parent) :
 		_menu(0),
 		_trajectory(new pcl::PointCloud<pcl::PointXYZ>),
 		_maxTrajectorySize(100),
-		_workingDirectory(".")
+		_workingDirectory("."),
+		_backgroundColor(Qt::black)
 {
 	this->setMinimumSize(200, 200);
 
@@ -657,8 +658,14 @@ void CloudViewer::render()
 	this->GetRenderWindow()->Render();
 }
 
+const QColor & CloudViewer::getBackgroundColor() const
+{
+	return _backgroundColor;
+}
+
 void CloudViewer::setBackgroundColor(const QColor & color)
 {
+	_backgroundColor = color;
 	_visualizer->setBackgroundColor(color.redF(), color.greenF(), color.blueF());
 }
 

guilib/src/MainWindow.cpp

@@ -161,6 +161,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 		_ui->dockWidget_loopClosureViewer->setVisible(false);
 		_ui->dockWidget_mapVisibility->setVisible(false);
 		_ui->dockWidget_graphViewer->setVisible(false);
+		_ui->dockWidget_odometry->setVisible(false);
 		//_ui->dockWidget_cloudViewer->setVisible(false);
 		//_ui->dockWidget_imageView->setVisible(false);
 	}
@@ -194,6 +195,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	//Graphics scenes
 	_ui->imageView_source->setBackgroundBrush(QBrush(Qt::black));
 	_ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::black));
+	_ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::black));
 
 	_posteriorCurve = new PdfPlotCurve("Posterior", &_cachedSignatures, this);
 	_ui->posteriorPlot->addCurve(_posteriorCurve, false);
@@ -240,6 +242,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_ui->menuShow_view->addAction(_ui->dockWidget_loopClosureViewer->toggleViewAction());
 	_ui->menuShow_view->addAction(_ui->dockWidget_mapVisibility->toggleViewAction());
 	_ui->menuShow_view->addAction(_ui->dockWidget_graphViewer->toggleViewAction());
+	_ui->menuShow_view->addAction(_ui->dockWidget_odometry->toggleViewAction());
 	_ui->menuShow_view->addAction(_ui->toolBar->toggleViewAction());
 	_ui->toolBar->setWindowTitle(tr("Control toolbar"));
 	QAction * a = _ui->menuShow_view->addAction("Progress dialog");
@@ -455,6 +458,7 @@ void MainWindow::closeEvent(QCloseEvent* event)
 		_ui->dockWidget_loopClosureViewer->close();
 		_ui->dockWidget_mapVisibility->close();
 		_ui->dockWidget_graphViewer->close();
+		_ui->dockWidget_odometry->close();
 
 		if(_camera)
 		{
@@ -561,7 +565,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
 	else if(anEvent->getClassName().compare("OdometryEvent") == 0)
 	{
 		OdometryEvent * odomEvent = (OdometryEvent*)anEvent;
-		if(_ui->dockWidget_cloudViewer->isVisible() &&
+		if((_ui->dockWidget_cloudViewer->isVisible() || _ui->dockWidget_odometry->isVisible()) &&
 		   _lastOdometryProcessed &&
 		   !_processingStatistics)
 		{
@@ -592,12 +596,16 @@ void MainWindow::handleEvent(UEvent* anEvent)
 void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality, float time, int features, int localMapSize)
 {
 	Transform pose = data.pose();
+	bool lost = false;
+	_ui->imageView_odometry->resetTransform();
 	if(pose.isNull())
 	{
 		UDEBUG("odom lost"); // use last pose
 		_ui->widget_cloudViewer->setBackgroundColor(Qt::darkRed);
+		_ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::darkRed));
 
 		pose = _lastOdomPose;
+		lost = true;
 	}
 	else if(quality>=0 &&
 			_preferencesDialog->getOdomQualityWarnThr() &&
@@ -605,11 +613,13 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality,
 	{
 		UDEBUG("odom warn, quality=%d thr=%d", quality, _preferencesDialog->getOdomQualityWarnThr());
 		_ui->widget_cloudViewer->setBackgroundColor(Qt::darkYellow);
+		_ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::darkYellow));
 	}
 	else
 	{
 		UDEBUG("odom ok");
 		_ui->widget_cloudViewer->setBackgroundColor(Qt::black);
+		_ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::black));
 	}
 	if(quality >= 0)
 	{
@@ -627,65 +637,94 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality,
 	{
 		_ui->statsToolBox->updateStat("Odometry/LocalMapSize/", (float)data.id(), (float)localMapSize);
 	}
-	if(!pose.isNull())
+
+	if(_ui->dockWidget_cloudViewer->isVisible())
 	{
-		_lastOdomPose = pose;
+		if(!pose.isNull())
-		_odometryReceived = true;
-
-		// 3d cloud
-		if(data.depth().cols == data.image().cols &&
-		   data.depth().rows == data.image().rows &&
-		   !data.depth().empty() &&
-		   data.fx() > 0.0f &&
-		   data.fy() > 0.0f &&
-		   _preferencesDialog->isCloudsShown(1))
 		{
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+			_lastOdomPose = pose;
-			cloud = createCloud(0,
+			_odometryReceived = true;
-					data.image(),
-					data.depth(),
-					data.fx(),
-					data.fy(),
-					data.cx(),
-					data.cy(),
-					data.localTransform(),
-					pose,
-					_preferencesDialog->getCloudVoxelSize(1),
-					_preferencesDialog->getCloudDecimation(1),
-					_preferencesDialog->getCloudMaxDepth(1));
 
-			if(!_ui->widget_cloudViewer->addOrUpdateCloud("cloudOdom", cloud, _odometryCorrection))
+			// 3d cloud
+			if(data.depth().cols == data.image().cols &&
+			   data.depth().rows == data.image().rows &&
+			   !data.depth().empty() &&
+			   data.fx() > 0.0f &&
+			   data.fy() > 0.0f &&
+			   _preferencesDialog->isCloudsShown(1))
 			{
-				UERROR("Adding cloudOdom to viewer failed!");
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+				cloud = createCloud(0,
+						data.image(),
+						data.depth(),
+						data.fx(),
+						data.fy(),
+						data.cx(),
+						data.cy(),
+						data.localTransform(),
+						pose,
+						_preferencesDialog->getCloudVoxelSize(1),
+						_preferencesDialog->getCloudDecimation(1),
+						_preferencesDialog->getCloudMaxDepth(1));
+
+				if(!_ui->widget_cloudViewer->addOrUpdateCloud("cloudOdom", cloud, _odometryCorrection))
+				{
+					UERROR("Adding cloudOdom to viewer failed!");
+				}
+				_ui->widget_cloudViewer->setCloudVisibility("cloudOdom", true);
+				_ui->widget_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
+				_ui->widget_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
 			}
-			_ui->widget_cloudViewer->setCloudVisibility("cloudOdom", true);
+
-			_ui->widget_cloudViewer->setCloudOpacity("cloudOdom", _preferencesDialog->getCloudOpacity(1));
+			// 2d cloud
-			_ui->widget_cloudViewer->setCloudPointSize("cloudOdom", _preferencesDialog->getCloudPointSize(1));
+			if(!data.depth2d().empty() &&
+				_preferencesDialog->isScansShown(1))
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+				cloud = util3d::depth2DToPointCloud(data.depth2d());
+				cloud = util3d::transformPointCloud<pcl::PointXYZ>(cloud, pose);
+				if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection))
+				{
+					UERROR("Adding scanOdom to viewer failed!");
+				}
+				_ui->widget_cloudViewer->setCloudVisibility("scanOdom", true);
+				_ui->widget_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
+				_ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
+			}
+
+			if(!data.pose().isNull())
+			{
+				// update camera position
+				_ui->widget_cloudViewer->updateCameraPosition(_odometryCorrection*data.pose());
+			}
+		}
+		_ui->widget_cloudViewer->render();
+	}
+
+	if(_ui->dockWidget_odometry->isVisible() &&
+	   !data.image().empty())
+	{
+		if(lost)
+		{
+			_ui->imageView_odometry->setImageDepth(uCvMat2QImage(data.image()));
+			_ui->imageView_odometry->setImageShown(true);
+			_ui->imageView_odometry->setImageDepthShown(true);
+		}
+		else
+		{
+			_ui->imageView_odometry->setImage(uCvMat2QImage(data.image()));
+			_ui->imageView_odometry->setImageShown(true);
+			_ui->imageView_odometry->setImageDepthShown(false);
 		}
 
-		// 2d cloud
+		_ui->imageView_odometry->resetZoom();
-		if(!data.depth2d().empty() &&
+		_ui->imageView_odometry->setSceneRect(_ui->imageView_odometry->scene()->itemsBoundingRect());
-			_preferencesDialog->isScansShown(1))
+		_ui->imageView_odometry->fitInView(_ui->imageView_odometry->sceneRect(), Qt::KeepAspectRatio);
+		if(_preferencesDialog->isImageFlipped())
 		{
-			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+			_ui->imageView_odometry->scale(-1.0, 1.0);
-			cloud = util3d::depth2DToPointCloud(data.depth2d());
-			cloud = util3d::transformPointCloud<pcl::PointXYZ>(cloud, pose);
-			if(!_ui->widget_cloudViewer->addOrUpdateCloud("scanOdom", cloud, _odometryCorrection))
-			{
-				UERROR("Adding scanOdom to viewer failed!");
-			}
-			_ui->widget_cloudViewer->setCloudVisibility("scanOdom", true);
-			_ui->widget_cloudViewer->setCloudOpacity("scanOdom", _preferencesDialog->getScanOpacity(1));
-			_ui->widget_cloudViewer->setCloudPointSize("scanOdom", _preferencesDialog->getScanPointSize(1));
-		}
-
-		if(!data.pose().isNull())
-		{
-			// update camera position
-			_ui->widget_cloudViewer->updateCameraPosition(_odometryCorrection*data.pose());
 		}
 	}
-	_ui->widget_cloudViewer->render();
 
 	_lastOdometryProcessed = true;
 
@@ -1885,8 +1924,10 @@ void MainWindow::resizeEvent(QResizeEvent* anEvent)
 {
 	_ui->imageView_source->fitInView(_ui->imageView_source->sceneRect(), Qt::KeepAspectRatio);
 	_ui->imageView_loopClosure->fitInView(_ui->imageView_source->sceneRect(), Qt::KeepAspectRatio);
+	_ui->imageView_source->fitInView(_ui->imageView_odometry->sceneRect(), Qt::KeepAspectRatio);
 	_ui->imageView_source->resetZoom();
 	_ui->imageView_loopClosure->resetZoom();
+	_ui->imageView_odometry->resetZoom();
 }
 
 void MainWindow::updateSelectSourceImageMenu(int type)
@@ -3266,10 +3307,13 @@ void MainWindow::clearTheCache()
 	_ui->graphicsView_graphView->clearAll();
 	_ui->imageView_source->clear();
 	_ui->imageView_loopClosure->clear();
+	_ui->imageView_odometry->clear();
 	_ui->imageView_source->resetTransform();
 	_ui->imageView_loopClosure->resetTransform();
+	_ui->imageView_odometry->resetTransform();
 	_ui->imageView_source->setBackgroundBrush(QBrush(Qt::black));
 	_ui->imageView_loopClosure->setBackgroundBrush(QBrush(Qt::black));
+	_ui->imageView_odometry->setBackgroundBrush(QBrush(Qt::black));
 }
 
 void MainWindow::updateElapsedTime()

guilib/src/ui/mainWindow.ui

@@ -716,6 +716,27 @@
     </layout>
    </widget>
   </widget>
+  <widget class="QDockWidget" name="dockWidget_odometry">
+   <property name="windowTitle">
+    <string>Odometry</string>
+   </property>
+   <attribute name="dockWidgetArea">
+    <number>1</number>
+   </attribute>
+   <widget class="QWidget" name="dockWidgetContents_10">
+    <layout class="QVBoxLayout" name="verticalLayout_11">
+     <property name="spacing">
+      <number>0</number>
+     </property>
+     <property name="margin">
+      <number>0</number>
+     </property>
+     <item>
+      <widget class="rtabmap::ImageView" name="imageView_odometry"/>
+     </item>
+    </layout>
+   </widget>
+  </widget>
   <action name="actionExit">
    <property name="text">
     <string>Exit</string>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap</name>
-  <version>0.7.2</version>
+  <version>0.7.3</version>
   <description>RTAB-Map's standalone library. RTAB-Map is an RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>