From b8542f754b32a55d76aa67df81c513cfc6a987d1 Mon Sep 17 00:00:00 2001 From: MarcLeclercGit Date: Fri, 7 May 2021 11:24:52 -0400 Subject: [PATCH 1/2] add flood fill filter --- corelib/include/rtabmap/core/OctoMap.h | 3 +- corelib/src/OctoMap.cpp | 103 ++++++++++++++++++++----- 2 files changed, 86 insertions(+), 20 deletions(-) diff --git a/corelib/include/rtabmap/core/OctoMap.h b/corelib/include/rtabmap/core/OctoMap.h index 2734a08419..b3f67db2f7 100644 --- a/corelib/include/rtabmap/core/OctoMap.h +++ b/corelib/include/rtabmap/core/OctoMap.h @@ -193,7 +193,8 @@ class RTABMAP_EXP OctoMap { std::vector * groundIndices = 0, bool originalRefPoints = true, std::vector * frontierIndices = 0, - std::vector * cloudProb = 0) const; + std::vector * cloudProb = 0, + bool applyFloodFill = false ) const; cv::Mat createProjectionMap( float & xMin, diff --git a/corelib/src/OctoMap.cpp b/corelib/src/OctoMap.cpp index a3b76c54d0..67430d3f02 100644 --- a/corelib/src/OctoMap.cpp +++ b/corelib/src/OctoMap.cpp @@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include namespace rtabmap { @@ -887,6 +888,52 @@ void OctoMap::updateMinMax(const octomap::point3d & point) } } + +bool isNodeVisited(std::unordered_multimap const & EmptyNodes,octomap::OcTreeKey const key) +{ + for(auto it = EmptyNodes.find(key);it != EmptyNodes.end();it++) + { + if(it->first == key) + { + return true; + } + + } + return false; +} + +void floodFill(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition, std::unordered_multimap & 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_multimap findEmptyNode(RtabmapColorOcTree* octree_, unsigned int treeDepth, octomap::point3d startPosition) +{ + std::unordered_multimap EmptyNodes; + floodFill(octree_,treeDepth,startPosition,EmptyNodes); + return EmptyNodes; +} + + pcl::PointCloud::Ptr OctoMap::createCloud( unsigned int treeDepth, std::vector * obstacleIndices, @@ -894,7 +941,8 @@ pcl::PointCloud::Ptr OctoMap::createCloud( std::vector * groundIndices, bool originalRefPoints, std::vector * frontierIndices, - std::vector * cloudProb) const + std::vector * cloudProb, + bool applyFloodFill) const { UASSERT(treeDepth <= octree_->getTreeDepth()); pcl::PointCloud::Ptr cloud(new pcl::PointCloud); @@ -937,6 +985,18 @@ pcl::PointCloud::Ptr OctoMap::createCloud( int fi=0; int gi=0; float halfCellSize = octree_->getNodeSize(treeDepth)/2.0f; + + std::unordered_multimap 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)) @@ -997,25 +1057,30 @@ pcl::PointCloud::Ptr OctoMap::createCloud( (*cloudProb)[oi] = it->getOccupancy(); } - 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; - } - - - (*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(!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 ? + { + //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(); + + if(emptyIndices) + { + emptyIndices->at(ei++) = oi; + } + + } ++oi; } } From 5a4acb15ac6a599f6f600b334dece80d40fbab09 Mon Sep 17 00:00:00 2001 From: MarcLeclercGit Date: Fri, 7 May 2021 16:10:21 -0400 Subject: [PATCH 2/2] change to use unordoned_multiset --- corelib/src/OctoMap.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/corelib/src/OctoMap.cpp b/corelib/src/OctoMap.cpp index 67430d3f02..74959ca0ee 100644 --- a/corelib/src/OctoMap.cpp +++ b/corelib/src/OctoMap.cpp @@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include namespace rtabmap { @@ -889,11 +890,11 @@ void OctoMap::updateMinMax(const octomap::point3d & point) } -bool isNodeVisited(std::unordered_multimap const & EmptyNodes,octomap::OcTreeKey const key) +bool isNodeVisited(std::unordered_multiset const & EmptyNodes,octomap::OcTreeKey const key) { for(auto it = EmptyNodes.find(key);it != EmptyNodes.end();it++) { - if(it->first == key) + if(*it == key) { return true; } @@ -902,7 +903,7 @@ bool isNodeVisited(std::unordered_multimap & EmptyNodes) +void floodFill(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::point3d startPosition, std::unordered_multiset & EmptyNodes) { auto key = octree_->coordToKey(startPosition,treeDepth); if(!isNodeVisited(EmptyNodes,key)) @@ -926,9 +927,9 @@ void floodFill(RtabmapColorOcTree* octree_, unsigned int treeDepth,octomap::poin } -std::unordered_multimap findEmptyNode(RtabmapColorOcTree* octree_, unsigned int treeDepth, octomap::point3d startPosition) +std::unordered_multiset findEmptyNode(RtabmapColorOcTree* octree_, unsigned int treeDepth, octomap::point3d startPosition) { - std::unordered_multimap EmptyNodes; + std::unordered_multiset EmptyNodes; floodFill(octree_,treeDepth,startPosition,EmptyNodes); return EmptyNodes; } @@ -986,7 +987,7 @@ pcl::PointCloud::Ptr OctoMap::createCloud( int gi=0; float halfCellSize = octree_->getNodeSize(treeDepth)/2.0f; - std::unordered_multimap EmptyNodes; + std::unordered_multiset EmptyNodes; if(applyFloodFill) {