Parameters in group GridGlobal: renamed OctoMapOccupancyThr to OccupancyThr, added ProbHit, ProbMiss, ProbClampingMin, ProbClampingMax. Using logodds approach from OctoMap to evaluate occupancy of standard grid map. ref https://github.com/introlab/rtabmap_ros/issues/269

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -39,6 +39,17 @@ namespace rtabmap {
 
 class RTABMAP_EXP OccupancyGrid
 {
+public:
+	inline static float logodds(double probability)
+	{
+		return (float) log(probability/(1-probability));
+	}
+
+	inline static double probability(double logodds)
+	{
+		return 1. - ( 1. / (1. + exp(logodds)));
+	}
+
 public:
 	OccupancyGrid(const ParametersMap & parameters = ParametersMap());
 	void parseParameters(const ParametersMap & parameters);
@@ -88,6 +99,7 @@ public:
 			const cv::Mat & empty);
 	void update(const std::map<int, Transform> & poses);
 	cv::Mat getMap(float & xMin, float & yMin) const;
+	cv::Mat getProbMap(float & xMin, float & yMin) const;
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapGround() const {return assembledGround_;}
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapObstacles() const {return assembledObstacles_;}
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapEmptyCells() const {return assembledEmptyCells_;}
@@ -126,6 +138,11 @@ private:
 	bool erode_;
 	float footprintRadius_;
 	float updateError_;
+	float occupancyThr_;
+	float probHit_;
+	float probMiss_;
+	float probClampingMin_;
+	float probClampingMax_;
 
 	std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > cache_; //<node id, < <ground, obstacles>, empty> >
 	cv::Mat map_;

corelib/include/rtabmap/core/Parameters.h

@@ -678,7 +678,11 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(GridGlobal, MinSize,              float,  0.0,     "Minimum map size (m).");
     RTABMAP_PARAM(GridGlobal, Eroded,               bool,   false,   "Erode obstacle cells.");
     RTABMAP_PARAM(GridGlobal, MaxNodes,             int,    0,       "Maximum nodes assembled in the map starting from the last node (0=unlimited).");
-    RTABMAP_PARAM(GridGlobal, OctoMapOccupancyThr,  float,  0.5,     "OctoMap occupancy threshold (value between 0 and 1).");
+    RTABMAP_PARAM(GridGlobal, OccupancyThr,         float,  0.5,     "Occupancy threshold (value between 0 and 1).");
+    RTABMAP_PARAM(GridGlobal, ProbHit,              float,  0.7,     "Probability of a hit (value between 0.5 and 1).");
+    RTABMAP_PARAM(GridGlobal, ProbMiss,             float,  0.4,     "Probability of a miss (value between 0 and 0.5).");
+    RTABMAP_PARAM(GridGlobal, ProbClampingMin,      float,  0.1192,  "Probability clamping minimum (value between 0 and 1).");
+    RTABMAP_PARAM(GridGlobal, ProbClampingMax,      float,  0.971,   "Probability clamping maximum (value between 0 and 1).");
 
 public:
     virtual ~Parameters();

corelib/src/OccupancyGrid.cpp

@@ -74,6 +74,11 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
 	erode_(Parameters::defaultGridGlobalEroded()),
 	footprintRadius_(Parameters::defaultGridGlobalFootprintRadius()),
 	updateError_(Parameters::defaultGridGlobalUpdateError()),
+	occupancyThr_(Parameters::defaultGridGlobalOccupancyThr()),
+	probHit_(Parameters::defaultGridGlobalProbHit()),
+	probMiss_(Parameters::defaultGridGlobalProbMiss()),
+	probClampingMin_(Parameters::defaultGridGlobalProbClampingMin()),
+	probClampingMax_(Parameters::defaultGridGlobalProbClampingMax()),
 	xMin_(0.0f),
 	yMin_(0.0f),
 	cloudAssembling_(false),
@@ -131,6 +136,20 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kGridGlobalFootprintRadius(), footprintRadius_);
 	Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_);
 
+	Parameters::parse(parameters, Parameters::kGridGlobalOccupancyThr(), occupancyThr_);
+	Parameters::parse(parameters, Parameters::kGridGlobalProbHit(), probHit_);
+	Parameters::parse(parameters, Parameters::kGridGlobalProbMiss(), probMiss_);
+	Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMin(), probClampingMin_);
+	Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMax(), probClampingMax_);
+	probHit_ = logodds(probHit_);
+	UASSERT(probHit_ >= 0.0f);
+	probMiss_ = logodds(probMiss_);
+	UASSERT(probMiss_ <= 0.0f);
+	probClampingMin_ = logodds(probClampingMin_);
+	probClampingMax_ = logodds(probClampingMax_);
+	UASSERT(probClampingMax_ > probClampingMin_);
+
+
 	UASSERT(minMapSize_ >= 0.0f);
 
 	// convert ROI from string to vector
@@ -205,7 +224,23 @@ void OccupancyGrid::setMap(const cv::Mat & map, float xMin, float yMin, float ce
 		UASSERT(cellSize > 0.0f);
 		UASSERT(map.type() == CV_8SC1);
 		map_ = map.clone();
-		mapInfo_ = cv::Mat::zeros(map.size(), CV_32FC3);
+		mapInfo_ = cv::Mat::zeros(map.size(), CV_32FC4);
+		for(int i=0; i<map_.rows; ++i)
+		{
+			for(int j=0; j<map_.cols; ++j)
+			{
+				const char value = map_.at<char>(i,j);
+				float * info = mapInfo_.ptr<float>(i,j);
+				if(value == 0)
+				{
+					info[3] = probClampingMin_;
+				}
+				else if(value == 100)
+				{
+					info[3] = probClampingMax_;
+				}
+			}
+		}
 		xMin_ = xMin;
 		yMin_ = yMin;
 		cellSize_ = cellSize;
@@ -563,11 +598,71 @@ cv::Mat OccupancyGrid::getMap(float & xMin, float & yMin) const
 {
 	xMin = xMin_;
 	yMin = yMin_;
-	if(erode_ && !map_.empty())
+
+	cv::Mat map = map_;
+
+	UTimer t;
+	if(occupancyThr_ != 0.0f && !map.empty())
 	{
-		return util3d::erodeMap(map_);
+		float occThr = logodds(occupancyThr_);
+		map = cv::Mat(map.size(), map.type());
+		UASSERT(mapInfo_.cols == map.cols && mapInfo_.rows == map.rows);
+		for(int i=0; i<map.rows; ++i)
+		{
+			for(int j=0; j<map.cols; ++j)
+			{
+				const float * info = mapInfo_.ptr<float>(i, j);
+				if(info[3] == 0.0f)
+				{
+					map.at<char>(i, j) = -1; // unknown
+				}
+				else if(info[3] >= occThr)
+				{
+					map.at<char>(i, j) = 100; // unknown
+				}
+				else
+				{
+					map.at<char>(i, j) = 0; // empty
+				}
+			}
+		}
+		UDEBUG("Converting map from probabilities (thr=%f) = %fs", occupancyThr_, t.ticks());
 	}
-	return map_;
+
+	if(erode_ && !map.empty())
+	{
+		map = util3d::erodeMap(map);
+		UDEBUG("Eroding map = %fs", t.ticks());
+	}
+	return map;
+}
+
+cv::Mat OccupancyGrid::getProbMap(float & xMin, float & yMin) const
+{
+	xMin = xMin_;
+	yMin = yMin_;
+
+	cv::Mat map;
+	if(!mapInfo_.empty())
+	{
+		map = cv::Mat(mapInfo_.size(), map_.type());
+		for(int i=0; i<map.rows; ++i)
+		{
+			for(int j=0; j<map.cols; ++j)
+			{
+				const float * info = mapInfo_.ptr<float>(i, j);
+				if(info[3] == 0.0f)
+				{
+					map.at<char>(i, j) = -1; // unknown
+				}
+				else
+				{
+					map.at<char>(i, j) = char(probability(info[3])*100.0f); // empty
+				}
+			}
+		}
+	}
+	return map;
 }
 
 void OccupancyGrid::addToCache(
@@ -996,7 +1091,7 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 			{
 				UDEBUG("Map empty!");
 				map = cv::Mat::ones(newMapSize, CV_8S)*-1;
-				mapInfo = cv::Mat::zeros(newMapSize, CV_32FC3);
+				mapInfo = cv::Mat::zeros(newMapSize, CV_32FC4);
 			}
 			else
 			{
@@ -1110,6 +1205,20 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 								cter->second.first+=1;
 							}
 							value = 0; // free space
+
+							// update odds
+							if(nodeId != kter->first)
+							{
+								info[3] += probMiss_;
+								if (info[3] < probClampingMin_)
+								{
+									info[3] = probClampingMin_;
+								}
+								if (info[3] > probClampingMax_)
+								{
+									info[3] = probClampingMax_;
+								}
+							}
 						}
 					}
 				}
@@ -1220,6 +1329,20 @@ void OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 								cter->second.second+=1;
 							}
 							value = 100; // obstacles
+
+							// update odds
+							if(nodeId != kter->first)
+							{
+								info[3] += probHit_;
+								if (info[3] < probClampingMin_)
+								{
+									info[3] = probClampingMin_;
+								}
+								if (info[3] > probClampingMax_)
+								{
+									info[3] = probClampingMax_;
+								}
+							}
 						}
 					}
 				}

corelib/src/OctoMap.cpp

@@ -276,11 +276,30 @@ OctoMap::OctoMap(const ParametersMap & parameters) :
 	minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
 	maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
 
-	float occupancyThr = Parameters::defaultGridGlobalOctoMapOccupancyThr();
+	float occupancyThr = Parameters::defaultGridGlobalOccupancyThr();
-	Parameters::parse(parameters, Parameters::kGridGlobalOctoMapOccupancyThr(), occupancyThr);
+	float probHit = Parameters::defaultGridGlobalProbHit();
+	float probMiss = Parameters::defaultGridGlobalProbMiss();
+	float clampingMin = Parameters::defaultGridGlobalProbClampingMin();
+	float clampingMax = Parameters::defaultGridGlobalProbClampingMax();
+	Parameters::parse(parameters, Parameters::kGridGlobalOccupancyThr(), occupancyThr);
+	Parameters::parse(parameters, Parameters::kGridGlobalProbHit(), probHit);
+	Parameters::parse(parameters, Parameters::kGridGlobalProbMiss(), probMiss);
+	Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMin(), clampingMin);
+	Parameters::parse(parameters, Parameters::kGridGlobalProbClampingMax(), clampingMax);
 
 	octree_ = new RtabmapColorOcTree(cellSize);
+	if(occupancyThr <= 0.0f)
+	{
+		UWARN("Cannot set %s to null for OctoMap, using default value %f instead.",
+				Parameters::kGridGlobalOccupancyThr().c_str(),
+				Parameters::defaultGridGlobalOccupancyThr());
+		occupancyThr = Parameters::defaultGridGlobalOccupancyThr();
+	}
 	octree_->setOccupancyThres(occupancyThr);
+	octree_->setProbHit(probHit);
+	octree_->setProbMiss(probMiss);
+	octree_->setClampingThresMin(clampingMin);
+	octree_->setClampingThresMax(clampingMax);
 	Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
 	Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_);
 	Parameters::parse(parameters, Parameters::kGridRangeMax(), rangeMax_);

corelib/src/Parameters.cpp

@@ -227,6 +227,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters
 
+		// 0.17.5
+		removedParameters_.insert(std::make_pair("Grid/OctoMapOccupancyThr",     std::make_pair(true,  Parameters::kGridGlobalOccupancyThr())));
+
 		// 0.17.0
 		removedParameters_.insert(std::make_pair("Grid/Scan2dMaxFilledRange",     std::make_pair(false,  Parameters::kGridRangeMax())));
 

guilib/src/PreferencesDialog.cpp

@@ -994,7 +994,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->doubleSpinBox_grid_minMapSize->setObjectName(Parameters::kGridGlobalMinSize().c_str());
 	_ui->spinBox_grid_maxNodes->setObjectName(Parameters::kGridGlobalMaxNodes().c_str());
 	_ui->doubleSpinBox_grid_footprintRadius->setObjectName(Parameters::kGridGlobalFootprintRadius().c_str());
-	_ui->doubleSpinBox_grid_octomapOccThr->setObjectName(Parameters::kGridGlobalOctoMapOccupancyThr().c_str());
+	_ui->doubleSpinBox_grid_occThr->setObjectName(Parameters::kGridGlobalOccupancyThr().c_str());
+	_ui->doubleSpinBox_grid_probHit->setObjectName(Parameters::kGridGlobalProbHit().c_str());
+	_ui->doubleSpinBox_grid_probMiss->setObjectName(Parameters::kGridGlobalProbMiss().c_str());
+	_ui->doubleSpinBox_grid_clampingMin->setObjectName(Parameters::kGridGlobalProbClampingMin().c_str());
+	_ui->doubleSpinBox_grid_clampingMax->setObjectName(Parameters::kGridGlobalProbClampingMax().c_str());
 	_ui->checkBox_grid_erode->setObjectName(Parameters::kGridGlobalEroded().c_str());
 
 	//Odometry

guilib/src/ui/preferencesDialog.ui

@@ -95,15 +95,24 @@
            <rect>
             <x>0</x>
             <y>0</y>
-            <width>673</width>
+            <width>681</width>
-            <height>2803</height>
+            <height>2943</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
            <property name="spacing">
             <number>0</number>
            </property>
-           <property name="margin">
+           <property name="leftMargin">
+            <number>0</number>
+           </property>
+           <property name="topMargin">
+            <number>0</number>
+           </property>
+           <property name="rightMargin">
+            <number>0</number>
+           </property>
+           <property name="bottomMargin">
             <number>0</number>
            </property>
            <item>
@@ -117,7 +126,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>12</number>
+                <number>16</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -5084,7 +5093,16 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        <string>Directory of images (optional settings)</string>
                       </property>
                       <layout class="QVBoxLayout" name="verticalLayout_93">
-                       <property name="margin">
+                       <property name="leftMargin">
+                        <number>0</number>
+                       </property>
+                       <property name="topMargin">
+                        <number>0</number>
+                       </property>
+                       <property name="rightMargin">
+                        <number>0</number>
+                       </property>
+                       <property name="bottomMargin">
                         <number>0</number>
                        </property>
                        <item>
@@ -10405,6 +10423,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                    <layout class="QVBoxLayout" name="verticalLayout_115">
                     <item>
                      <layout class="QGridLayout" name="gridLayout_88" columnstretch="0,1">
+                      <item row="8" column="1">
+                       <widget class="QLabel" name="label_484">
+                        <property name="text">
+                         <string>Probability clamping threshold minimum.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="0" column="0">
                        <widget class="QCheckBox" name="checkBox_grid_fullUpdate">
                         <property name="text">
@@ -10428,7 +10459,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="6" column="1">
+                      <item row="10" column="1">
                        <widget class="QLabel" name="label_224">
                         <property name="text">
                          <string>Erode obstacle cells.</string>
@@ -10441,6 +10472,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="6" column="1">
+                       <widget class="QLabel" name="label_482">
+                        <property name="text">
+                         <string>Probability of a hit.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="4" column="1">
                        <widget class="QLabel" name="label_319">
                         <property name="text">
@@ -10454,7 +10498,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="6" column="0">
+                      <item row="10" column="0">
                        <widget class="QCheckBox" name="checkBox_grid_erode">
                         <property name="text">
                          <string/>
@@ -10576,7 +10620,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       <item row="5" column="1">
                        <widget class="QLabel" name="label_457">
                         <property name="text">
-                         <string>OctoMap occupancy threshold.</string>
+                         <string>Occupancy threshold.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -10587,7 +10631,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </widget>
                       </item>
                       <item row="5" column="0">
-                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_octomapOccThr">
+                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_occThr">
                         <property name="suffix">
                          <string/>
                         </property>
@@ -10605,6 +10649,108 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="7" column="1">
+                       <widget class="QLabel" name="label_483">
+                        <property name="text">
+                         <string>Probability of a miss.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="6" column="0">
+                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_probHit">
+                        <property name="suffix">
+                         <string/>
+                        </property>
+                        <property name="minimum">
+                         <double>0.500000000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.100000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.500000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="7" column="0">
+                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_probMiss">
+                        <property name="suffix">
+                         <string/>
+                        </property>
+                        <property name="minimum">
+                         <double>0.000000000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>0.500000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.100000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.500000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="9" column="1">
+                       <widget class="QLabel" name="label_485">
+                        <property name="text">
+                         <string>Probability clamping threshold maximum.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="8" column="0">
+                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_clampingMin">
+                        <property name="suffix">
+                         <string/>
+                        </property>
+                        <property name="minimum">
+                         <double>0.000000000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.100000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.500000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="9" column="0">
+                       <widget class="QDoubleSpinBox" name="doubleSpinBox_grid_clampingMax">
+                        <property name="suffix">
+                         <string/>
+                        </property>
+                        <property name="minimum">
+                         <double>0.000000000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.100000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.500000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                    </layout>
@@ -15124,7 +15270,16 @@ Lower the ratio -&gt; higher the precision.</string>
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="margin">
+                        <property name="leftMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="topMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="rightMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -15204,7 +15359,16 @@ Lower the ratio -&gt; higher the precision.</string>
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="margin">
+                        <property name="leftMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="topMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="rightMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>
@@ -15316,7 +15480,16 @@ Lower the ratio -&gt; higher the precision.</string>
                         <property name="spacing">
                          <number>0</number>
                         </property>
-                        <property name="margin">
+                        <property name="leftMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="topMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="rightMargin">
+                         <number>0</number>
+                        </property>
+                        <property name="bottomMargin">
                          <number>0</number>
                         </property>
                         <item>