Added notification message box when a global path is computed. Refactoring occupancy grid.

corelib/src/util3d.cpp

@@ -2595,12 +2595,32 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
 	cv::Mat affineTransform(2,3,CV_32FC1);
 	for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 	{
+		UASSERT(!iter->second.isNull());
+
+		iter->second.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+
+		if(undefinedSize)
+		{
+			minX = maxX = x;
+			minY = maxY = y;
+			undefinedSize = false;
+		}
+		else
+		{
+			if(minX > x)
+				minX = x;
+			else if(maxX < x)
+				maxX = x;
+
+			if(minY > y)
+				minY = y;
+			else if(maxY < y)
+				maxY = y;
+		}
+
 		if(uContains(occupancy, iter->first))
 		{
-			UASSERT(!iter->second.isNull());
 			const std::pair<cv::Mat, cv::Mat> & pair = occupancy.at(iter->first);
-
-			iter->second.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
 			cosT = cos(yaw);
 			sinT = sin(yaw);
 			affineTransform.at<float>(0,0) = cosT;
@@ -2610,25 +2630,6 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
 			affineTransform.at<float>(0,2) = x;
 			affineTransform.at<float>(1,2) = y;
 
-			if(undefinedSize)
-			{
-				minX = maxX = x;
-				minY = maxY = y;
-				undefinedSize = false;
-			}
-			else
-			{
-				if(minX > x)
-					minX = x;
-				else if(maxX < x)
-					maxX = x;
-
-				if(minY > y)
-					minY = y;
-				else if(maxY < y)
-					maxY = y;
-			}
-
 			//ground
 			if(pair.first.rows)
 			{

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -89,6 +89,7 @@ private slots:
 	void refineAllLoopClosureLinks();
 	void refineVisuallyAllNeighborLinks();
 	void refineVisuallyAllLoopClosureLinks();
+	void resetAllChanges();
 	void sliderAValueChanged(int);
 	void sliderBValueChanged(int);
 	void sliderAMoved(int);

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -124,6 +124,7 @@ public:
 	bool imageRejectedShown() const;
 	bool imageHighestHypShown() const;
 	bool beepOnPause() const;
+	bool notifyWhenNewGlobalPathIsReceived() const;
 	int getOdomQualityWarnThr() const;
 	bool isPosteriorGraphView() const;
 

guilib/src/DatabaseViewer.cpp

@@ -151,6 +151,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	connect(ui_->actionRefine_all_loop_closure_links, SIGNAL(triggered()), this, SLOT(refineAllLoopClosureLinks()));
 	connect(ui_->actionVisual_Refine_all_neighbor_links, SIGNAL(triggered()), this, SLOT(refineVisuallyAllNeighborLinks()));
 	connect(ui_->actionVisual_Refine_all_loop_closure_links, SIGNAL(triggered()), this, SLOT(refineVisuallyAllLoopClosureLinks()));
+	connect(ui_->actionReset_all_changes, SIGNAL(triggered()), this, SLOT(resetAllChanges()));
 
 	//ICP buttons
 	connect(ui_->pushButton_refine, SIGNAL(clicked()), this, SLOT(refineConstraint()));
@@ -627,6 +628,11 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
 void DatabaseViewer::showEvent(QShowEvent* anEvent)
 {
 	this->setWindowModified(false);
+
+	if(ui_->graphViewer->isVisible() && graphes_.size() && localMaps_.size()==0)
+	{
+		sliderIterationsValueChanged((int)graphes_.size()-1);
+	}
 }
 
 void DatabaseViewer::moveEvent(QMoveEvent* anEvent)
@@ -1367,6 +1373,15 @@ void DatabaseViewer::refineVisuallyAllLoopClosureLinks()
 	}
 }
 
+void DatabaseViewer::resetAllChanges()
+{
+	linksAdded_.clear();
+	linksRefined_.clear();
+	linksRemoved_.clear();
+	updateLoopClosuresSlider();
+	this->updateGraphView();
+}
+
 void DatabaseViewer::sliderAValueChanged(int value)
 {
 	this->update(value,

guilib/src/GraphViewer.cpp

@@ -396,6 +396,10 @@ void GraphViewer::updateReferentialPosition(const Transform & t)
 	_localRadius->setTransform(qt);
 
 	this->ensureVisible(_referential);
+	if(_localRadius->isVisible())
+	{
+		this->ensureVisible(_localRadius, 0, 0);
+	}
 }
 
 void GraphViewer::updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin)

guilib/src/MainWindow.cpp

@@ -1479,27 +1479,13 @@ void MainWindow::updateMapCloud(
 	{
 		float xMin, yMin;
 		float resolution = _preferencesDialog->getGridMapResolution();
-		cv::Mat map8S;
+		cv::Mat map8S = util3d::create2DMapFromOccupancyLocalMaps(
-		if(_preferencesDialog->isGridMapFrom3DCloud())
-		{
-			map8S = util3d::create2DMapFromOccupancyLocalMaps(
 					poses,
-					_projectionLocalMaps,
+					_preferencesDialog->isGridMapFrom3DCloud()?_projectionLocalMaps:_gridLocalMaps,
 					resolution,
 					xMin, yMin,
 					0,
 					_preferencesDialog->isGridMapEroded());
-		}
-		else if(_gridLocalMaps.size())
-		{
-			map8S = util3d::create2DMapFromOccupancyLocalMaps(
-					poses,
-					_gridLocalMaps,
-					resolution,
-					xMin, yMin,
-					0,
-					_preferencesDialog->isGridMapEroded());
-		}
 		if(!map8S.empty())
 		{
 			//convert to gray scaled map
@@ -1977,20 +1963,40 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 
 void MainWindow::processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathEvent & event)
 {
-	if(event.getPoses().empty())
+	if(!event.getPoses().empty())
-	{
-		QMessageBox::warning(this, tr("Setting goal failed"), tr("Setting goal to location %1 failed. "
-				"Some reasons: \n"
-				"1) the location doesn't exist in the graph,\n"
-				"2) the location is not linked to the global graph or\n"
-				"3) the location is too near of the current location (goal already reached).").arg(event.getGoal()));
-	}
-	else
 	{
 		_ui->graphicsView_graphView->setGlobalPath(event.getPoses());
-		_ui->statusbar->showMessage(
+	}
-				tr("Global path computed from %1 (%2 poses, %3 m)!").arg(event.getGoal()).arg(event.getPoses().size()).arg(graph::computePathLength(event.getPoses())),
+
-				10000);
+	if(_preferencesDialog->notifyWhenNewGlobalPathIsReceived())
+	{
+		// use MessageBox
+		if(event.getPoses().empty())
+		{
+			QMessageBox * warn = new QMessageBox(
+					QMessageBox::Warning,
+					tr("Setting goal failed!"),
+					tr("Setting goal to location %1 failed. "
+						"Some reasons: \n"
+						"1) the location doesn't exist in the graph,\n"
+						"2) the location is not linked to the global graph or\n"
+						"3) the location is too near of the current location (goal already reached).").arg(event.getGoal()),
+					QMessageBox::Ok,
+					this);
+			warn->setAttribute(Qt::WA_DeleteOnClose, true);
+			warn->show();
+		}
+		else
+		{
+			QMessageBox * info = new QMessageBox(
+					QMessageBox::Information,
+					tr("Goal detected!"),
+					tr("Global path computed from %1 to %2 (%3 poses, %4 m).").arg(event.getPoses().front().first).arg(event.getGoal()).arg(event.getPoses().size()).arg(graph::computePathLength(event.getPoses())),
+					QMessageBox::Ok,
+					this);
+			info->setAttribute(Qt::WA_DeleteOnClose, true);
+			info->show();
+		}
 	}
 }
 
@@ -4038,27 +4044,13 @@ void MainWindow::exportGridMap()
 
 	// create the map
 	float xMin=0.0f, yMin=0.0f;
-	cv::Mat pixels;
+	cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps(
-	if(_preferencesDialog->isGridMapFrom3DCloud())
-	{
-		pixels = util3d::create2DMapFromOccupancyLocalMaps(
 				poses,
-				_projectionLocalMaps,
+				_preferencesDialog->isGridMapFrom3DCloud()?_projectionLocalMaps:_gridLocalMaps,
 				gridCellSize,
 				xMin, yMin,
 				0,
 				_preferencesDialog->isGridMapEroded());
-	}
-	else
-	{
-		pixels = util3d::create2DMapFromOccupancyLocalMaps(
-				poses,
-				_gridLocalMaps,
-				gridCellSize,
-				xMin, yMin,
-				0,
-				_preferencesDialog->isGridMapEroded());
-	}
 
 	if(!pixels.empty())
 	{

guilib/src/PreferencesDialog.cpp

@@ -192,6 +192,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->checkBox_imageRejectedShown, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkBox_imageHighestHypShown, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkBox_beep, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
+	connect(_ui->checkBox_notifyWhenNewGlobalPathIsReceived, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->spinBox_odomQualityWarnThr, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkBox_posteriorGraphView, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 
@@ -881,6 +882,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 	{
 		_ui->general_checkBox_imagesKept->setChecked(true);
 		_ui->checkBox_beep->setChecked(false);
+		_ui->checkBox_notifyWhenNewGlobalPathIsReceived->setChecked(false);
 		_ui->checkBox_verticalLayoutUsed->setChecked(true);
 		_ui->checkBox_imageRejectedShown->setChecked(true);
 		_ui->checkBox_imageHighestHypShown->setChecked(false);
@@ -1143,6 +1145,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->checkBox_imageRejectedShown->setChecked(settings.value("imageRejectedShown", _ui->checkBox_imageRejectedShown->isChecked()).toBool());
 	_ui->checkBox_imageHighestHypShown->setChecked(settings.value("imageHighestHypShown", _ui->checkBox_imageHighestHypShown->isChecked()).toBool());
 	_ui->checkBox_beep->setChecked(settings.value("beep", _ui->checkBox_beep->isChecked()).toBool());
+	_ui->checkBox_notifyWhenNewGlobalPathIsReceived->setChecked(settings.value("notifyNewGlobalPath", _ui->checkBox_notifyWhenNewGlobalPathIsReceived->isChecked()).toBool());
 	_ui->spinBox_odomQualityWarnThr->setValue(settings.value("odomQualityThr", _ui->spinBox_odomQualityWarnThr->value()).toInt());
 	_ui->checkBox_posteriorGraphView->setChecked(settings.value("posteriorGraphView", _ui->checkBox_posteriorGraphView->isChecked()).toBool());
 
@@ -1412,6 +1415,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 	settings.setValue("imageRejectedShown", _ui->checkBox_imageRejectedShown->isChecked());
 	settings.setValue("imageHighestHypShown", _ui->checkBox_imageHighestHypShown->isChecked());
 	settings.setValue("beep", _ui->checkBox_beep->isChecked());
+	settings.setValue("notifyNewGlobalPath", _ui->checkBox_notifyWhenNewGlobalPathIsReceived->isChecked());
 	settings.setValue("odomQualityThr", _ui->spinBox_odomQualityWarnThr->value());
 	settings.setValue("posteriorGraphView", _ui->checkBox_posteriorGraphView->isChecked());
 
@@ -2868,6 +2872,10 @@ bool PreferencesDialog::beepOnPause() const
 {
 	return _ui->checkBox_beep->isChecked();
 }
+bool PreferencesDialog::notifyWhenNewGlobalPathIsReceived() const
+{
+	return _ui->checkBox_notifyWhenNewGlobalPathIsReceived->isChecked();
+}
 int PreferencesDialog::getOdomQualityWarnThr() const
 {
 	return _ui->spinBox_odomQualityWarnThr->value();

guilib/src/ui/DatabaseViewer.ui

@@ -414,6 +414,8 @@
     <addaction name="actionVisual_Refine_all_loop_closure_links"/>
     <addaction name="actionRefine_all_loop_closure_links"/>
     <addaction name="separator"/>
+    <addaction name="actionReset_all_changes"/>
+    <addaction name="separator"/>
     <addaction name="actionView_3D_map"/>
     <addaction name="actionGenerate_3D_map_pcd"/>
    </widget>
@@ -1756,6 +1758,11 @@
     <string>Save config</string>
    </property>
   </action>
+  <action name="actionReset_all_changes">
+   <property name="text">
+    <string>Reset all changes</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>

guilib/src/ui/preferencesDialog.ui

@@ -65,7 +65,7 @@
             <x>0</x>
             <y>0</y>
             <width>760</width>
-            <height>1502</height>
+            <height>1029</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>19</number>
+                <number>0</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -135,6 +135,26 @@
                         </property>
                        </widget>
                       </item>
+                      <item row="2" column="1">
+                       <widget class="QLabel" name="label_102">
+                        <property name="text">
+                         <string>Notify when a new global path is received.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="2" column="0">
+                       <widget class="QCheckBox" name="checkBox_notifyWhenNewGlobalPathIsReceived">
+                        <property name="text">
+                         <string/>
+                        </property>
+                        <property name="checked">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>