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,6 +637,9 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality,
 	{
 		_ui->statsToolBox->updateStat("Odometry/LocalMapSize/", (float)data.id(), (float)localMapSize);
 	}
+
+	if(_ui->dockWidget_cloudViewer->isVisible())
+	{
 		if(!pose.isNull())
 		{
 			_lastOdomPose = pose;
@@ -686,6 +699,32 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, int quality,
 			}
 		}
 		_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);
+		}
+
+		_ui->imageView_odometry->resetZoom();
+		_ui->imageView_odometry->setSceneRect(_ui->imageView_odometry->scene()->itemsBoundingRect());
+		_ui->imageView_odometry->fitInView(_ui->imageView_odometry->sceneRect(), Qt::KeepAspectRatio);
+		if(_preferencesDialog->isImageFlipped())
+		{
+			_ui->imageView_odometry->scale(-1.0, 1.0);
+		}
+	}
 
 	_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>