Fixed #695 (footprint not cleared correctly when GridGlobal/OccupancyThr>0)

corelib/src/OccupancyGrid.cpp

@@ -669,6 +669,10 @@ cv::Mat OccupancyGrid::getProbMap(float & xMin, float & yMin) const
 			}
 		}
 	}
+	else
+	{
+		UWARN("Map info is empty, cannot generate probabilistic occupancy grid");
+	}
 	return map;
 }
 
@@ -1244,6 +1248,7 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 							ptBegin.y = 0;
 						if(ptEnd.y >= map.rows)
 							ptEnd.y = map.rows-1;
+
 						for(int i=ptBegin.x; i<ptEnd.x; ++i)
 						{
 							for(int j=ptBegin.y; j<ptEnd.y; ++j)
@@ -1282,6 +1287,7 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 									info[0] = (float)kter->first;
 									info[1] = float(i) * cellSize_ + xMin;
 									info[2] = float(j) * cellSize_ + yMin;
+									info[3] = probClampingMin_;
 									cter->second.first+=1;
 								}
 								value = -2; // free space (footprint)

corelib/src/util3d_mapping.cpp

@@ -932,10 +932,18 @@ cv::Mat convertMap2Image8U(const cv::Mat & map8S, bool pgmFormat)
 			{
 				gray = pgmFormat?254:200;
 			}
-			else // -1
+			else if(pgmFormat || v == -1)// -1
 			{
 				gray = pgmFormat?205:89;
 			}
+			else if(v>50)
+			{
+				gray = double(100-v)*2/100.0*double(89);
+			}
+			else // v<50
+			{
+				gray = double(50-v)*2/100.0*double(178-89)+89;
+			}
 			map8U.at<unsigned char>(i, j) = gray;
 		}
 	}

guilib/src/DatabaseViewer.cpp

@@ -443,6 +443,8 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->toolButton_groundColor, SIGNAL(clicked(bool)), this, SLOT(selectGroundColor()));
 	connect(ui_->toolButton_emptyColor, SIGNAL(clicked(bool)), this, SLOT(selectEmptyColor()));
 	connect(ui_->spinBox_cropRadius, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
+	connect(ui_->checkBox_grid_showProbMap, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
+	connect(ui_->checkBox_grid_showProbMap, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 
 	connect(exportDialog_, SIGNAL(configChanged()), this, SLOT(configModified()));
 
@@ -555,6 +557,7 @@ void DatabaseViewer::readSettings()
 	ui_->lineEdit_groundColor->setText(settings.value("colorGround", ui_->lineEdit_groundColor->text()).toString());
 	ui_->lineEdit_emptyColor->setText(settings.value("colorEmpty", ui_->lineEdit_emptyColor->text()).toString());
 	ui_->spinBox_cropRadius->setValue(settings.value("cropRadius", ui_->spinBox_cropRadius->value()).toInt());
+	ui_->checkBox_grid_showProbMap->setChecked(settings.value("probMap", ui_->checkBox_grid_showProbMap->isChecked()).toBool());
 	settings.endGroup();
 
 	settings.beginGroup("mesh");
@@ -640,6 +643,7 @@ void DatabaseViewer::writeSettings()
 	settings.setValue("colorGround", ui_->lineEdit_groundColor->text());
 	settings.setValue("colorEmpty", ui_->lineEdit_emptyColor->text());
 	settings.setValue("cropRadius", ui_->spinBox_cropRadius->value());
+	settings.setValue("probMap", ui_->checkBox_grid_showProbMap->isChecked());
 	settings.endGroup();
 
 	settings.beginGroup("mesh");
@@ -726,6 +730,7 @@ void DatabaseViewer::restoreDefaultSettings()
 	ui_->lineEdit_groundColor->setText(QColor(Qt::green).name());
 	ui_->lineEdit_emptyColor->setText(QColor(Qt::yellow).name());
 	ui_->spinBox_cropRadius->setValue(1);
+	ui_->checkBox_grid_showProbMap->setChecked(false);
 
 	ui_->checkBox_mesh_quad->setChecked(true);
 	ui_->spinBox_mesh_angleTolerance->setValue(15);
@@ -6411,7 +6416,14 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 					for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter)
 					{
 						grid.addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second);
-					}
+					}
+					grid.update(graphFiltered);
+					if(ui_->checkBox_grid_showProbMap->isChecked())
+					{
+						map = grid.getProbMap(xMin, yMin);
+					}
+					else
+					{
 						map = grid.getMap(xMin, yMin);
 					}
 				}

guilib/src/ui/DatabaseViewer.ui

@@ -61,7 +61,7 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>419</width>
+           <width>311</width>
            <height>311</height>
           </rect>
          </property>
@@ -304,7 +304,7 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>418</width>
+           <width>311</width>
            <height>311</height>
           </rect>
          </property>
@@ -1379,7 +1379,7 @@
      <item>
       <widget class="QToolBox" name="toolBox">
        <property name="currentIndex">
-        <number>0</number>
+        <number>1</number>
        </property>
        <widget class="QWidget" name="page_3">
         <property name="geometry">
@@ -1541,9 +1541,9 @@
         <property name="geometry">
          <rect>
           <x>0</x>
-          <y>0</y>
+          <y>-330</y>
-          <width>280</width>
+          <width>519</width>
-          <height>949</height>
+          <height>871</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1552,6 +1552,16 @@
         <layout class="QVBoxLayout" name="verticalLayout_16">
          <item>
           <layout class="QGridLayout" name="gridLayout_9" columnstretch="0,0">
+           <item row="7" column="1">
+            <widget class="QLabel" name="label_octomap_cubes">
+             <property name="text">
+              <string>OctoMap: Rendering type</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
            <item row="10" column="1">
             <widget class="QLabel" name="label_51">
              <property name="text">
@@ -1666,16 +1676,6 @@
              </property>
             </widget>
            </item>
-           <item row="7" column="1">
-            <widget class="QLabel" name="label_octomap_cubes">
-             <property name="text">
-              <string>OctoMap: Rendering type</string>
-             </property>
-             <property name="wordWrap">
-              <bool>true</bool>
-             </property>
-            </widget>
-           </item>
            <item row="7" column="0">
             <widget class="QComboBox" name="comboBox_octomap_rendering_type">
              <item>
@@ -1855,6 +1855,26 @@
              </property>
             </widget>
            </item>
+           <item row="14" column="1">
+            <widget class="QLabel" name="label_59">
+             <property name="text">
+              <string>Show probabilistic occupancy grid in Graph View.</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="14" column="0">
+            <widget class="QCheckBox" name="checkBox_grid_showProbMap">
+             <property name="text">
+              <string/>
+             </property>
+             <property name="checked">
+              <bool>false</bool>
+             </property>
+            </widget>
+           </item>
           </layout>
          </item>
          <item>