Grid map: added "erode" option

corelib/include/rtabmap/core/util3d.h

@@ -358,7 +358,8 @@ cv::Mat RTABMAP_EXP create2DMapFromOccupancyLocalMaps(
 		float cellSize,
 		float & xMin,
 		float & yMin,
-		float minMapSize = 0.0f);
+		float minMapSize = 0.0f,
+		bool erode = false);
 
 cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses,
 		const std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > & scans,

corelib/src/util3d.cpp

@@ -2100,6 +2100,8 @@ void occupancy2DFromLaserScan(
  * @param cellSize m
  * @param xMin
  * @param yMin
+ * @param minMapSize minimum width (m)
+ * @param erode
  */
 cv::Mat create2DMapFromOccupancyLocalMaps(
 		const std::map<int, Transform> & poses,
@@ -2107,10 +2109,11 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
 		float cellSize,
 		float & xMin,
 		float & yMin,
-		float minMapSize)
+		float minMapSize,
+		bool erode)
 {
 	UASSERT(minMapSize >= 0.0f);
-	UDEBUG("");
+	UDEBUG("cellSize=%f m, minMapSize=%f m, erode=%d", cellSize, minMapSize, erode?1:0);
 	UTimer timer;
 
 	std::map<int, cv::Mat> emptyLocalMaps;
@@ -2248,7 +2251,8 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
 			}
 
 			// fill holes and remove empty from obstacle borders
-			cv::Mat updatedMap = map;
+			cv::Mat updatedMap = map.clone();
+			std::list<std::pair<int, int> > obstacleIndices;
 			for(int i=2; i<map.rows-2; ++i)
 			{
 				for(int j=2; j<map.cols-2; ++j)
@@ -2285,11 +2289,55 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
 						{
 							updatedMap.at<char>(i, j+1) = -1;
 						}
+
+						if(erode)
+						{
+							obstacleIndices.push_back(std::make_pair(i, j));
+						}
+					}
+					else if(map.at<char>(i, j) == 0)
+					{
+						// obstacle/empty/obstacle -> remove empty
+						if(map.at<char>(i-1, j) == 100 &&
+							map.at<char>(i+1, j) == 100)
+						{
+							updatedMap.at<char>(i, j) = -1;
+						}
+						else if(map.at<char>(i, j-1) == 100 &&
+							map.at<char>(i, j+1) == 100)
+						{
+							updatedMap.at<char>(i, j) = -1;
+						}
 					}
 
 				}
 			}
 			map = updatedMap;
+
+			if(erode)
+			{
+				// remove obstacles which touch to empty cells but not unknown cells
+				cv::Mat erodedMap = map.clone();
+				for(std::list<std::pair<int,int> >::iterator iter = obstacleIndices.begin();
+					iter!= obstacleIndices.end();
+					++iter)
+				{
+					int i = iter->first;
+					int j = iter->second;
+					bool touchEmpty = map.at<char>(i+1, j) == 0 ||
+						map.at<char>(i-1, j) == 0 ||
+						map.at<char>(i, j+1) == 0 ||
+						map.at<char>(i, j-1) == 0;
+					if(touchEmpty && map.at<char>(i+1, j) != -1 &&
+						map.at<char>(i-1, j) != -1 &&
+						map.at<char>(i, j+1) != -1 &&
+						map.at<char>(i, j-1) != -1)
+					{
+						erodedMap.at<char>(i, j) = 0; // empty
+					}
+				}
+				map = erodedMap;
+			}
 		}
 	}
 	UDEBUG("timer=%fs", timer.ticks());

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -149,6 +149,7 @@ public:
 	bool getGridMapShown() const;
 	double getGridMapResolution() const;
 	bool isGridMapFrom3DCloud() const;
+	bool isGridMapEroded() const;
 	double getGridMapOpacity() const;
 
 	QString getWorkingDirectory() const;

guilib/src/DatabaseViewer.cpp

@@ -195,7 +195,10 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	connect(ui_->comboBox_graphOptimizer, SIGNAL(currentIndexChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_2dslam, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->spinBox_optimizationDepth, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
-
+	connect(ui_->checkBox_gridErode, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
+	connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(updateGraphView()));
+	connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
+	connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
 
 	connect(ui_->groupBox_gridFromProjection, SIGNAL(clicked(bool)), this, SLOT(updateGrid()));
 	connect(ui_->doubleSpinBox_gridCellSize, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
@@ -213,6 +216,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
 	connect(ui_->comboBox_graphOptimizer, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_2dslam, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->spinBox_optimizationDepth, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
+	connect(ui_->checkBox_gridErode, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->groupBox_gridFromProjection, SIGNAL(clicked(bool)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_gridCellSize, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->spinBox_projDecimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
@@ -313,6 +317,7 @@ void DatabaseViewer::readSettings()
 	ui_->comboBox_graphOptimizer->setCurrentIndex(settings.value("strategy", ui_->comboBox_graphOptimizer->currentIndex()).toInt());
 	ui_->checkBox_2dslam->setChecked(settings.value("slam2d", ui_->checkBox_2dslam->isChecked()).toBool());
 	ui_->spinBox_optimizationDepth->setValue(settings.value("depth", ui_->spinBox_optimizationDepth->value()).toInt());
+	ui_->checkBox_gridErode->setChecked(settings.value("erode", ui_->checkBox_gridErode->isChecked()).toBool());
 	settings.endGroup();
 
 	settings.beginGroup("grid");
@@ -385,6 +390,7 @@ void DatabaseViewer::writeSettings()
 	settings.setValue("strategy", ui_->comboBox_graphOptimizer->currentIndex());
 	settings.setValue("slam2d", ui_->checkBox_2dslam->isChecked());
 	settings.setValue("depth", ui_->spinBox_optimizationDepth->value());
+	settings.setValue("erode", ui_->checkBox_gridErode->isChecked());
 	settings.endGroup();
 
 	// save Grid settings
@@ -2242,21 +2248,24 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 			float xMin, yMin;
 			float cell = ui_->doubleSpinBox_gridCellSize->value();
 			cv::Mat map;
+			QTime time;
+			time.start();
 			if(ui_->groupBox_posefiltering->isChecked())
 			{
 				std::map<int, rtabmap::Transform> graphFiltered = graph::radiusPosesFiltering(graph,
 						ui_->doubleSpinBox_posefilteringRadius->value(),
 						ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
-				map = rtabmap::util3d::create2DMapFromOccupancyLocalMaps(graphFiltered, localMaps_, cell, xMin, yMin);
+				map = rtabmap::util3d::create2DMapFromOccupancyLocalMaps(graphFiltered, localMaps_, cell, xMin, yMin, 0, ui_->checkBox_gridErode->isChecked());
 			}
 			else
 			{
-				map = rtabmap::util3d::create2DMapFromOccupancyLocalMaps(graph, localMaps_, cell, xMin, yMin);
+				map = rtabmap::util3d::create2DMapFromOccupancyLocalMaps(graph, localMaps_, cell, xMin, yMin, 0, ui_->checkBox_gridErode->isChecked());
 			}
 			if(!map.empty())
 			{
 				ui_->graphViewer->updateMap(rtabmap::util3d::convertMap2Image8U(map), cell, xMin, yMin);
 			}
+			ui_->label_timeGrid->setNum(double(time.elapsed())/1000.0);
 		}
 		ui_->graphViewer->update();
 		ui_->label_iterations->setNum(value);

guilib/src/MainWindow.cpp

@@ -1438,7 +1438,9 @@ void MainWindow::updateMapCloud(
 					poses,
 					_projectionLocalMaps,
 					resolution,
-					xMin, yMin);
+					xMin, yMin,
+					0,
+					_preferencesDialog->isGridMapEroded());
 		}
 		else if(_gridLocalMaps.size())
 		{
@@ -1446,7 +1448,9 @@ void MainWindow::updateMapCloud(
 					poses,
 					_gridLocalMaps,
 					resolution,
-					xMin, yMin);
+					xMin, yMin,
+					0,
+					_preferencesDialog->isGridMapEroded());
 		}
 		if(!map8S.empty())
 		{
@@ -3948,7 +3952,9 @@ void MainWindow::exportGridMap()
 				poses,
 				_projectionLocalMaps,
 				gridCellSize,
-				xMin, yMin);
+				xMin, yMin,
+				0,
+				_preferencesDialog->isGridMapEroded());
 	}
 	else
 	{
@@ -3956,7 +3962,9 @@ void MainWindow::exportGridMap()
 				poses,
 				_gridLocalMaps,
 				gridCellSize,
-				xMin, yMin);
+				xMin, yMin,
+				0,
+				_preferencesDialog->isGridMapEroded());
 	}
 
 	if(!pixels.empty())

guilib/src/PreferencesDialog.cpp

@@ -237,6 +237,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->doubleSpinBox_map_resolution, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_map_opacity, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->checkBox_map_occupancyFrom3DCloud, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->checkBox_map_erode, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 
 	//Logging panel
 	connect(_ui->comboBox_loggerLevel, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteLoggingPanel()));
@@ -886,6 +887,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->checkBox_map_shown->setChecked(false);
 		_ui->doubleSpinBox_map_resolution->setValue(0.05);
 		_ui->checkBox_map_occupancyFrom3DCloud->setChecked(false);
+		_ui->checkBox_map_erode->setChecked(false);
 		_ui->doubleSpinBox_map_opacity->setValue(0.75);
 	}
 	else if(groupBox->objectName() == _ui->groupBox_logging1->objectName())
@@ -1136,6 +1138,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->checkBox_map_shown->setChecked(settings.value("gridMapShown", _ui->checkBox_map_shown->isChecked()).toBool());
 	_ui->doubleSpinBox_map_resolution->setValue(settings.value("gridMapResolution", _ui->doubleSpinBox_map_resolution->value()).toDouble());
 	_ui->checkBox_map_occupancyFrom3DCloud->setChecked(settings.value("gridMapOccupancyFrom3DCloud", _ui->checkBox_map_occupancyFrom3DCloud->isChecked()).toBool());
+	_ui->checkBox_map_erode->setChecked(settings.value("gridMapEroded", _ui->checkBox_map_erode->isChecked()).toBool());
 	_ui->doubleSpinBox_map_opacity->setValue(settings.value("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value()).toDouble());
 
 	settings.endGroup(); // General
@@ -1387,6 +1390,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 	settings.setValue("gridMapShown", _ui->checkBox_map_shown->isChecked());
 	settings.setValue("gridMapResolution", _ui->doubleSpinBox_map_resolution->value());
 	settings.setValue("gridMapOccupancyFrom3DCloud", _ui->checkBox_map_occupancyFrom3DCloud->isChecked());
+	settings.setValue("gridMapEroded", _ui->checkBox_map_erode->isChecked());
 	settings.setValue("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value());
 	settings.endGroup(); // General
 
@@ -2929,6 +2933,10 @@ bool PreferencesDialog::isGridMapFrom3DCloud() const
 {
 	return _ui->checkBox_map_occupancyFrom3DCloud->isChecked();
 }
+bool PreferencesDialog::isGridMapEroded() const
+{
+	return _ui->checkBox_map_erode->isChecked();
+}
 double PreferencesDialog::getGridMapOpacity() const
 {
 	return _ui->doubleSpinBox_map_opacity->value();

guilib/src/ui/DatabaseViewer.ui

@@ -630,6 +630,20 @@
          </property>
         </widget>
        </item>
+       <item row="4" column="1">
+        <widget class="QLabel" name="label_45">
+         <property name="text">
+          <string>Time grid (s)</string>
+         </property>
+        </widget>
+       </item>
+       <item row="4" column="0">
+        <widget class="QLabel" name="label_timeGrid">
+         <property name="text">
+          <string/>
+         </property>
+        </widget>
+       </item>
       </layout>
      </item>
     </layout>
@@ -1305,6 +1319,20 @@
          </property>
         </widget>
        </item>
+       <item row="1" column="1">
+        <widget class="QLabel" name="label_44">
+         <property name="text">
+          <string>Errode</string>
+         </property>
+        </widget>
+       </item>
+       <item row="1" column="0">
+        <widget class="QCheckBox" name="checkBox_gridErode">
+         <property name="text">
+          <string/>
+         </property>
+        </widget>
+       </item>
       </layout>
      </item>
      <item>

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-171</y>
+            <y>0</y>
             <width>744</width>
-            <height>1389</height>
+            <height>1110</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>1</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -421,6 +421,22 @@ Show a yellow background when the number of odometry inliers goes under this thr
                        </item>
                        <item>
                         <layout class="QGridLayout" name="gridLayout_20" columnstretch="0,1">
+                         <item row="2" column="0">
+                          <widget class="QDoubleSpinBox" name="doubleSpinBox_map_opacity">
+                           <property name="suffix">
+                            <string/>
+                           </property>
+                           <property name="minimum">
+                            <double>0.750000000000000</double>
+                           </property>
+                           <property name="maximum">
+                            <double>1.000000000000000</double>
+                           </property>
+                           <property name="value">
+                            <double>0.750000000000000</double>
+                           </property>
+                          </widget>
+                         </item>
                          <item row="0" column="0">
                           <widget class="QCheckBox" name="checkBox_map_shown">
                            <property name="text">
@@ -478,22 +494,6 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="0">
-                          <widget class="QDoubleSpinBox" name="doubleSpinBox_map_opacity">
-                           <property name="suffix">
-                            <string/>
-                           </property>
-                           <property name="minimum">
-                            <double>0.750000000000000</double>
-                           </property>
-                           <property name="maximum">
-                            <double>1.000000000000000</double>
-                           </property>
-                           <property name="value">
-                            <double>0.750000000000000</double>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="3" column="1">
                           <widget class="QLabel" name="label_210">
                            <property name="text">
@@ -504,6 +504,26 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
+                         <item row="4" column="1">
+                          <widget class="QLabel" name="label_224">
+                           <property name="text">
+                            <string>Erode.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="4" column="0">
+                          <widget class="QCheckBox" name="checkBox_map_erode">
+                           <property name="text">
+                            <string/>
+                           </property>
+                           <property name="checked">
+                            <bool>false</bool>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                       </layout>