i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-08 04:20:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
matlabbe
2018-08-16 14:42:05 -04:00
parent e0858a9c2a
commit 35d5200a3b
7 changed files with 364 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/OccupancyGrid.h
+17
View File
@@ -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
+5 -1
View File
@@ -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
+128 -5
View File
@@ -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
+21 -2
View File
@@ -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();
Parameters::parse(parameters, Parameters::kGridGlobalOctoMapOccupancyThr(), occupancyThr);
float occupancyThr = Parameters::defaultGridGlobalOccupancyThr();
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
+3
View File
@@ -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
+5 -1
View File
@@ -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
+185 -12
View File
@@ -95,15 +95,24 @@
<rect>
<x>0</x>
<y>0</y>
<width>673</width>
<height>2803</height>
<width>681</width>
<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>