i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +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

17
corelib/include/rtabmap/core/OccupancyGrid.h
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_;

6
corelib/include/rtabmap/core/Parameters.h
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();

133
corelib/src/OccupancyGrid.cpp
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_;
}
}
}
}
}

23
corelib/src/OctoMap.cpp
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_);

3
corelib/src/Parameters.cpp
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())));