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

improve flood fill and move it to update function (#727)

Browse Source 
* add flood fill filter

* change to use unordoned_multiset

* remove recursivity in flood fill and move the flood fill into the update

* Refactoring of OctoMap constructors (keeping only one). Simplified floodfill parameters to only one and added it to UI.

* Fixed ray tracing not done in local 3D maps (OctoMap)

Co-authored-by: MarcLeclercGit <marcantoine.leclerc96@gmail.com>
Co-authored-by: matlabbe <matlabbe@gmail.com>
This commit is contained in:
chameau5050
2021-05-30 10:12:07 -04:00
committed by GitHub
parent b7c2f6801b
commit f94f9cbb9b
7 changed files with 431 additions and 321 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
corelib/include/rtabmap/core/OctoMap.h
View File

@@ -40,7 +40,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/Parameters.h>
#include <map>
#include <unordered_set>
#include <string>
#include <queue>
namespace rtabmap {
@@ -169,8 +171,7 @@ class RtabmapColorOcTree : public octomap::OccupancyOcTreeBase <RtabmapColorOcTr
class RTABMAP_EXP OctoMap {
public:
OctoMap(const ParametersMap & parameters);
OctoMap(float cellSize = 0.1f, float occupancyThr = 0.5f, bool fullUpdate = false, float updateError=0.01f);
OctoMap(const ParametersMap & parameters = ParametersMap());
const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
void addToCache(int nodeId,
@@ -193,8 +194,7 @@ public:
std::vector<int> * groundIndices = 0,
bool originalRefPoints = true,
std::vector<int> * frontierIndices = 0,
std::vector<double> * cloudProb = 0,
bool applyFloodFill = false ) const;
std::vector<double> * cloudProb = 0) const;
cv::Mat createProjectionMap(
float & xMin,
@@ -215,6 +215,12 @@ public:
void setRayTracing(bool enabled) {rayTracing_ = enabled;}
bool hasColor() const {return hasColor_;}
static std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> findEmptyNode(RtabmapColorOcTree* octree_, unsigned int treeDepth, octomap::point3d startPosition);
static void floodFill(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition, std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> & EmptyNodes,std::queue<octomap::point3d>& positionToExplore);
static bool isNodeVisited(std::unordered_set<octomap::OcTreeKey,octomap::OcTreeKey::KeyHash> const & EmptyNodes,octomap::OcTreeKey const key);
static octomap::point3d findCloseEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition);
static bool isValidEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition);
private:
void updateMinMax(const octomap::point3d & point);
@@ -229,6 +235,7 @@ private:
float updateError_;
float rangeMax_;
bool rayTracing_;
unsigned int emptyFloodFillDepth_;
double minValues_[3];
double maxValues_[3];
};

1
corelib/include/rtabmap/core/Parameters.h
View File

@@ -764,6 +764,7 @@ class RTABMAP_EXP Parameters
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).");
RTABMAP_PARAM(GridGlobal, FloodFillDepth, unsigned int, 0, "Flood fill filter (0=disabled), used to remove empty cells outside the map. The flood fill is done at the specified depth (between 1 and 16) of the OctoMap.");
RTABMAP_PARAM(Marker, Dictionary, int, 0, "Dictionary to use: DICT_ARUCO_4X4_50=0, DICT_ARUCO_4X4_100=1, DICT_ARUCO_4X4_250=2, DICT_ARUCO_4X4_1000=3, DICT_ARUCO_5X5_50=4, DICT_ARUCO_5X5_100=5, DICT_ARUCO_5X5_250=6, DICT_ARUCO_5X5_1000=7, DICT_ARUCO_6X6_50=8, DICT_ARUCO_6X6_100=9, DICT_ARUCO_6X6_250=10, DICT_ARUCO_6X6_1000=11, DICT_ARUCO_7X7_50=12, DICT_ARUCO_7X7_100=13, DICT_ARUCO_7X7_250=14, DICT_ARUCO_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16, DICT_APRILTAG_16h5=17, DICT_APRILTAG_25h9=18, DICT_APRILTAG_36h10=19, DICT_APRILTAG_36h11=20");
RTABMAP_PARAM(Marker, Length, float, 0, "The length (m) of the markers' side. 0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization).");

7
corelib/src/OccupancyGrid.cpp
View File

@@ -529,8 +529,11 @@ void OccupancyGrid::createLocalMap(
if(!groundCloud.empty() || !obstaclesCloud.empty())
{
//create local octomap
OctoMap octomap(cellSize_);
octomap.setMaxRange(cloudMaxDepth_);
ParametersMap params;
params.insert(ParametersPair(Parameters::kGridCellSize(), uNumber2Str(cellSize_)));
params.insert(ParametersPair(Parameters::kGridRangeMax(), uNumber2Str(cloudMaxDepth_)));
params.insert(ParametersPair(Parameters::kGridRayTracing(), uNumber2Str(rayTracing_)));
OctoMap octomap(params);
octomap.addToCache(1, groundCloud, obstaclesCloud, cv::Mat(), cv::Point3f(viewPointInOut.x, viewPointInOut.y, viewPointInOut.z));
std::map<int, Transform> poses;
poses.insert(std::make_pair(1, Transform::getIdentity()));

268
corelib/src/OctoMap.cpp
View File

@@ -34,8 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/util3d_mapping.h>
#include <rtabmap/core/util2d.h>
#include <pcl/common/transforms.h>
#include <map>
#include <unordered_set>
namespace rtabmap {
@@ -275,7 +274,8 @@ OctoMap::OctoMap(const ParametersMap & parameters) :
fullUpdate_(Parameters::defaultGridGlobalFullUpdate()),
updateError_(Parameters::defaultGridGlobalUpdateError()),
rangeMax_(Parameters::defaultGridRangeMax()),
rayTracing_(Parameters::defaultGridRayTracing())
rayTracing_(Parameters::defaultGridRayTracing()),
emptyFloodFillDepth_(Parameters::defaultGridGlobalFloodFillDepth())
{
float cellSize = Parameters::defaultGridCellSize();
Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize);
@@ -309,24 +309,18 @@ OctoMap::OctoMap(const ParametersMap & parameters) :
octree_->setClampingThresMin(clampingMin);
octree_->setClampingThresMax(clampingMax);
Parameters::parse(parameters, Parameters::kGridGlobalFullUpdate(), fullUpdate_);
Parameters::parse(parameters, Parameters::kGridGlobalUpdateError(), updateError_);
Parameters::parse(parameters, Parameters::kGridRangeMax(), rangeMax_);
Parameters::parse(parameters, Parameters::kGridRayTracing(), rayTracing_);
}
Parameters::parse(parameters, Parameters::kGridGlobalFloodFillDepth(), emptyFloodFillDepth_);
UASSERT(emptyFloodFillDepth_>=0 && emptyFloodFillDepth_<=16);
OctoMap::OctoMap(float cellSize, float occupancyThr, bool fullUpdate, float updateError) :
octree_(new RtabmapColorOcTree(cellSize)),
hasColor_(false),
fullUpdate_(fullUpdate),
updateError_(updateError),
rangeMax_(0.0f),
rayTracing_(true)
{
minValues_[0] = minValues_[1] = minValues_[2] = 0.0;
maxValues_[0] = maxValues_[1] = maxValues_[2] = 0.0;
octree_->setOccupancyThres(occupancyThr);
UASSERT(cellSize>0.0f);
UDEBUG("fullUpdate_ =%s", fullUpdate_?"true":"false");
UDEBUG("updateError_ =%f", updateError_);
UDEBUG("rangeMax_ =%f", rangeMax_);
UDEBUG("rayTracing_ =%s", rayTracing_?"true":"false");
UDEBUG("emptyFloodFillDepth_=%d", emptyFloodFillDepth_);
}
OctoMap::~OctoMap()
@@ -381,6 +375,121 @@ void OctoMap::addToCache(int nodeId,
uInsert(cacheViewPoints_, std::make_pair(nodeId==0?-1:nodeId, viewPoint));
}
bool OctoMap::isValidEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition)
{
auto nodePtr = octree_->search(startPosition.x(), startPosition.y(), startPosition.z(), treeDepth);
if(nodePtr != NULL)
{
if(!octree_->isNodeOccupied(*nodePtr))
{
return true;
}
}
return false;
}
octomap::point3d OctoMap::findCloseEmpty(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition)
{
//try current position
if(isValidEmpty(octree_,treeDepth,startPosition))
{
return startPosition;
}
//x pos
if(isValidEmpty(octree_,treeDepth,octomap::point3d(startPosition.x()+octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z())))
{
return octomap::point3d(startPosition.x()+octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z());
}
//x neg
if(isValidEmpty(octree_,treeDepth,octomap::point3d(startPosition.x()-octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z())))
{
return octomap::point3d(startPosition.x()-octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z());
}
//y pos
if(isValidEmpty(octree_,treeDepth,octomap::point3d(startPosition.x(), startPosition.y()+octree_->getNodeSize(treeDepth), startPosition.z())))
{
return octomap::point3d(startPosition.x(), startPosition.y()+octree_->getNodeSize(treeDepth), startPosition.z());
}
//y neg
if(isValidEmpty(octree_,treeDepth,octomap::point3d(startPosition.x()-octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z())))
{
return octomap::point3d(startPosition.x(), startPosition.y()-octree_->getNodeSize(treeDepth), startPosition.z());
}
//z pos
if(isValidEmpty(octree_,treeDepth,octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()+octree_->getNodeSize(treeDepth))))
{
return octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()+octree_->getNodeSize(treeDepth));
}
//z neg
if(isValidEmpty(octree_,treeDepth,octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()-octree_->getNodeSize(treeDepth))))
{
return octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()-octree_->getNodeSize(treeDepth));
}
//no valid position
return startPosition;
}
bool OctoMap::isNodeVisited(std::unordered_set<octomap::OcTreeKey,octomap::OcTreeKey::KeyHash> const & EmptyNodes,octomap::OcTreeKey const key)
{
for(auto it = EmptyNodes.find(key);it != EmptyNodes.end();it++)
{
if(*it == key)
{
return true;
}
}
return false;
}
void OctoMap::floodFill(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition, std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> & EmptyNodes,std::queue<octomap::point3d>& positionToExplore)
{
auto key = octree_->coordToKey(startPosition,treeDepth);
if(!isNodeVisited(EmptyNodes,key))
{
if(isValidEmpty(octree_,treeDepth,startPosition))
{
EmptyNodes.insert(key);
positionToExplore.push(octomap::point3d(startPosition.x()+octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z()));
positionToExplore.push(octomap::point3d(startPosition.x()-octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z()));
positionToExplore.push(octomap::point3d(startPosition.x(), startPosition.y()+octree_->getNodeSize(treeDepth), startPosition.z()));
positionToExplore.push(octomap::point3d(startPosition.x(), startPosition.y()-octree_->getNodeSize(treeDepth), startPosition.z()));
positionToExplore.push(octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()+octree_->getNodeSize(treeDepth)));
positionToExplore.push(octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()-octree_->getNodeSize(treeDepth)));
}
}
}
std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> OctoMap::findEmptyNode(RtabmapColorOcTree* octree_, unsigned int treeDepth, octomap::point3d startPosition)
{
std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> exploreNode;
std::queue<octomap::point3d> positionToExplore;
startPosition = findCloseEmpty(octree_,treeDepth, startPosition);
floodFill(octree_, treeDepth, startPosition, exploreNode, positionToExplore);
while(!positionToExplore.empty())
{
floodFill(octree_, treeDepth, positionToExplore.front(), exploreNode, positionToExplore);
positionToExplore.pop();
}
return exploreNode;
}
bool OctoMap::update(const std::map<int, Transform> & poses)
{
UDEBUG("Update (poses=%d addedNodes_=%d)", (int)poses.size(), (int)addedNodes_.size());
@@ -852,13 +961,42 @@ bool OctoMap::update(const std::map<int, Transform> & poses)
}
}
if(emptyFloodFillDepth_>0)
{
UTimer t;
auto key = octree_->coordToKey(0, 0, 0, emptyFloodFillDepth_);
auto pos = octree_->keyToCoord(key);
std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> EmptyNodes = findEmptyNode(octree_,emptyFloodFillDepth_, pos);
std::vector<octomap::OcTreeKey> nodeToDelete;
for (RtabmapColorOcTree::iterator it = octree_->begin_leafs(emptyFloodFillDepth_); it != octree_->end_leafs(); ++it)
{
if(!octree_->isNodeOccupied(*it))
{
if(!isNodeVisited(EmptyNodes,it.getKey()))
{
nodeToDelete.push_back(it.getKey());
}
}
}
for(unsigned int y=0; y < nodeToDelete.size(); y++)
{
octree_->deleteNode(nodeToDelete[y],emptyFloodFillDepth_);
}
UDEBUG("Flood Fill: deleted %d empty cells (%fs)", (int)nodeToDelete.size(), t.ticks());
}
if(!fullUpdate_)
{
cache_.clear();
cacheClouds_.clear();
cacheViewPoints_.clear();
}
return !orderedPoses.empty() || graphOptimized || graphChanged;
return !orderedPoses.empty() || graphOptimized || graphChanged || emptyFloodFillDepth_>0;
}
void OctoMap::updateMinMax(const octomap::point3d & point)
@@ -890,51 +1028,6 @@ void OctoMap::updateMinMax(const octomap::point3d & point)
}
bool isNodeVisited(std::unordered_multiset<octomap::OcTreeKey,octomap::OcTreeKey::KeyHash> const & EmptyNodes,octomap::OcTreeKey const key)
{
for(auto it = EmptyNodes.find(key);it != EmptyNodes.end();it++)
{
if(*it == key)
{
return true;
}
}
return false;
}
void floodFill(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition, std::unordered_multiset<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> & EmptyNodes)
{
auto key = octree_->coordToKey(startPosition,treeDepth);
if(!isNodeVisited(EmptyNodes,key))
{
auto nodePtr = octree_->search(startPosition.x(), startPosition.y(), startPosition.z(), treeDepth);
if(nodePtr)
{
if(!octree_->isNodeOccupied(*nodePtr))
{
EmptyNodes.insert({key,key});
floodFill(octree_,treeDepth,octomap::point3d(startPosition.x()+octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z()),EmptyNodes);
floodFill(octree_,treeDepth,octomap::point3d(startPosition.x()-octree_->getNodeSize(treeDepth), startPosition.y(), startPosition.z()),EmptyNodes);
floodFill(octree_,treeDepth,octomap::point3d(startPosition.x(), startPosition.y()+octree_->getNodeSize(treeDepth), startPosition.z()),EmptyNodes);
floodFill(octree_,treeDepth,octomap::point3d(startPosition.x(), startPosition.y()-octree_->getNodeSize(treeDepth), startPosition.z()),EmptyNodes);
floodFill(octree_,treeDepth,octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()+octree_->getNodeSize(treeDepth)),EmptyNodes);
floodFill(octree_,treeDepth,octomap::point3d(startPosition.x(), startPosition.y(), startPosition.z()-octree_->getNodeSize(treeDepth)),EmptyNodes);
}
}
}
}
std::unordered_multiset<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> findEmptyNode(RtabmapColorOcTree* octree_, unsigned int treeDepth, octomap::point3d startPosition)
{
std::unordered_multiset<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> EmptyNodes;
floodFill(octree_,treeDepth,startPosition,EmptyNodes);
return EmptyNodes;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
unsigned int treeDepth,
std::vector<int> * obstacleIndices,
@@ -942,8 +1035,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
std::vector<int> * groundIndices,
bool originalRefPoints,
std::vector<int> * frontierIndices,
std::vector<double> * cloudProb,
bool applyFloodFill) const
std::vector<double> * cloudProb) const
{
UASSERT(treeDepth <= octree_->getTreeDepth());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
@@ -987,17 +1079,6 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
int gi=0;
float halfCellSize = octree_->getNodeSize(treeDepth)/2.0f;
std::unordered_multiset<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> EmptyNodes;
if(applyFloodFill)
{
auto key = octree_->coordToKey(0, 0, 1, treeDepth);
auto pos = octree_->keyToCoord(key);
EmptyNodes = findEmptyNode(octree_,treeDepth, pos);
}
for (RtabmapColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
{
if(octree_->isNodeOccupied(*it) && (obstacleIndices != 0 || groundIndices != 0 || addAllPoints))
@@ -1057,31 +1138,26 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
{
(*cloudProb)[oi] = it->getOccupancy();
}
if(!applyFloodFill || isNodeVisited(EmptyNodes,it.getKey()))
if(frontierIndices !=0 &&
(!octree_->search( pt.x()+octree_->getNodeSize(treeDepth), pt.y(), pt.z(), treeDepth) || !octree_->search( pt.x()-octree_->getNodeSize(treeDepth), pt.y(), pt.z(), treeDepth) ||
!octree_->search( pt.x(), pt.y()+octree_->getNodeSize(treeDepth), pt.z(), treeDepth) || !octree_->search( pt.x(), pt.y()-octree_->getNodeSize(treeDepth), pt.z(), treeDepth) ||
!octree_->search( pt.x(), pt.y(), pt.z()+octree_->getNodeSize(treeDepth), treeDepth) || !octree_->search( pt.x(), pt.y(), pt.z()-octree_->getNodeSize(treeDepth), treeDepth) )) //ajouter 1 au key ?
{
if(frontierIndices !=0 &&
(!octree_->search( pt.x()+octree_->getNodeSize(treeDepth), pt.y(), pt.z(), treeDepth) || !octree_->search( pt.x()-octree_->getNodeSize(treeDepth), pt.y(), pt.z(), treeDepth) ||
!octree_->search( pt.x(), pt.y()+octree_->getNodeSize(treeDepth), pt.z(), treeDepth) || !octree_->search( pt.x(), pt.y()-octree_->getNodeSize(treeDepth), pt.z(), treeDepth) ||
!octree_->search( pt.x(), pt.y(), pt.z()+octree_->getNodeSize(treeDepth), treeDepth) || !octree_->search( pt.x(), pt.y(), pt.z()-octree_->getNodeSize(treeDepth), treeDepth) )) //ajouter 1 au key ?
{
//unknown neighbor FACE cell
frontierIndices->at(fi++) = oi;
}
//unknown neighbor FACE cell
frontierIndices->at(fi++) = oi;
}
(*cloud)[oi] = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
(*cloud)[oi].x = pt.x()-halfCellSize;
(*cloud)[oi].y = pt.y()-halfCellSize;
(*cloud)[oi].z = pt.z();
(*cloud)[oi] = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
(*cloud)[oi].x = pt.x()-halfCellSize;
(*cloud)[oi].y = pt.y()-halfCellSize;
(*cloud)[oi].z = pt.z();
if(emptyIndices)
{
emptyIndices->at(ei++) = oi;
}
if(emptyIndices)
{
emptyIndices->at(ei++) = oi;
}
++oi;
}
}