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

OctoMap added frontier cloud output

Browse Source
This commit is contained in:
Louis Petit
2019-12-17 17:31:37 -05:00
parent af9e113112
commit af2b221958
2 changed files with 27 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -194,7 +194,8 @@ public:
std::vector<int> * obstacleIndices = 0, std::vector<int> * obstacleIndices = 0,
std::vector<int> * emptyIndices = 0, std::vector<int> * emptyIndices = 0,
std::vector<int> * groundIndices = 0, std::vector<int> * groundIndices = 0,
bool originalRefPoints = true) const; bool originalRefPoints = true,
std::vector<int> * frontierIndices = 0) const;
cv::Mat createProjectionMap( cv::Mat createProjectionMap(
float & xMin, float & xMin,

28
corelib/src/OctoMap.cpp
View File

@@ -940,7 +940,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
std::vector<int> * obstacleIndices, std::vector<int> * obstacleIndices,
std::vector<int> * emptyIndices, std::vector<int> * emptyIndices,
std::vector<int> * groundIndices, std::vector<int> * groundIndices,
bool originalRefPoints) const bool originalRefPoints,
std::vector<int> * frontierIndices) const
{ {
UASSERT(treeDepth <= octree_->getTreeDepth()); UASSERT(treeDepth <= octree_->getTreeDepth());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
@@ -955,6 +956,10 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
{ {
emptyIndices->resize(octree_->size()); emptyIndices->resize(octree_->size());
} }
if(frontierIndices)
{
frontierIndices->resize(octree_->size());
}
if(groundIndices) if(groundIndices)
{ {
groundIndices->resize(octree_->size()); groundIndices->resize(octree_->size());
@@ -972,6 +977,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
int oi=0; int oi=0;
int si=0; int si=0;
int ei=0; int ei=0;
int fi=0;
int gi=0; int gi=0;
float halfCellSize = octree_->getNodeSize(treeDepth)/2.0f; float halfCellSize = octree_->getNodeSize(treeDepth)/2.0f;
for (RtabmapColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it) for (RtabmapColorOcTree::iterator it = octree_->begin(treeDepth); it != octree_->end(); ++it)
@@ -1022,9 +1028,19 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
++oi; ++oi;
} }
else if(!octree_->isNodeOccupied(*it) && (emptyIndices != 0 || addAllPoints)) else if(!octree_->isNodeOccupied(*it) && (emptyIndices != 0 || addAllPoints || frontierIndices !=0))
{ {
octomap::point3d pt = octree_->keyToCoord(it.getKey()); octomap::point3d pt = octree_->keyToCoord(it.getKey());
if(frontierIndices !=0 && (!octree_->search( pt.x()+octree_->getNodeSize(treeDepth), pt.y(), pt.z() ) || !octree_->search( pt.x()-octree_->getNodeSize(treeDepth), pt.y(), pt.z() )
|| !octree_->search( pt.x(), pt.y()+octree_->getNodeSize(treeDepth), pt.z() ) || !octree_->search( pt.x(), pt.y()-octree_->getNodeSize(treeDepth), pt.z() )
|| !octree_->search( pt.x(), pt.y(), pt.z()+octree_->getNodeSize(treeDepth) ) || !octree_->search( pt.x(), pt.y(), pt.z()-octree_->getNodeSize(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] = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
(*cloud)[oi].x = pt.x()-halfCellSize; (*cloud)[oi].x = pt.x()-halfCellSize;
(*cloud)[oi].y = pt.y()-halfCellSize; (*cloud)[oi].y = pt.y()-halfCellSize;
@@ -1033,8 +1049,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
{ {
emptyIndices->at(ei++) = oi; emptyIndices->at(ei++) = oi;
} }
++oi; ++oi;
} }
} }
cloud->resize(oi); cloud->resize(oi);
@@ -1048,6 +1065,11 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
emptyIndices->resize(ei); emptyIndices->resize(ei);
UDEBUG("empty=%d", ei); UDEBUG("empty=%d", ei);
} }
if(frontierIndices)
{
frontierIndices->resize(fi);
UDEBUG("frontier=%d", fi);
}
if(groundIndices) if(groundIndices)
{ {
groundIndices->resize(gi); groundIndices->resize(gi);