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

Fixed unknown lines for octomap 2D grid projection. (https://github.com/introlab/rtabmap_ros/issues/684)

Browse Source
This commit is contained in:
matlabbe
2021-11-30 20:34:51 -05:00
parent 67710ef94c
commit baae713471
3 changed files with 30 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/OctoMap.h
+2
View File
@@ -74,6 +74,8 @@ public:
void expandNode(); void expandNode();
bool createChild(unsigned int i); bool createChild(unsigned int i);
void updateOccupancyTypeChildren();
private: private:
int nodeRefId_; int nodeRefId_;
int type_; // -1=undefined, 0=empty, 100=obstacle, 1=ground int type_; // -1=undefined, 0=empty, 100=obstacle, 1=ground
corelib/src/OctoMap.cpp
+26 -5
View File
@@ -106,6 +106,23 @@ bool RtabmapColorOcTreeNode::createChild(unsigned int i) {
#endif #endif
} }
void RtabmapColorOcTreeNode::updateOccupancyTypeChildren()
{
if (children != NULL){
int type = kTypeUnknown;
for (int i=0; i<8 && type != kTypeObstacle; i++) {
RtabmapColorOcTreeNode* child = static_cast<RtabmapColorOcTreeNode*>(children[i]);
if (child != NULL && child->getOccupancyType() >= kTypeEmpty) {
if(type == kTypeUnknown) {
type = child->getOccupancyType();
}
}
}
type_ = type;
}
}
RtabmapColorOcTree::RtabmapColorOcTree(double resolution) RtabmapColorOcTree::RtabmapColorOcTree(double resolution)
: OccupancyOcTreeBase<RtabmapColorOcTreeNode>(resolution) { : OccupancyOcTreeBase<RtabmapColorOcTreeNode>(resolution) {
RtabmapColorOcTreeMemberInit.ensureLinking(); RtabmapColorOcTreeMemberInit.ensureLinking();
@@ -231,6 +248,7 @@ void RtabmapColorOcTree::updateInnerOccupancyRecurs(RtabmapColorOcTreeNode* node
} }
node->updateOccupancyChildren(); node->updateOccupancyChildren();
node->updateColorChildren(); node->updateColorChildren();
node->updateOccupancyTypeChildren();
} }
#else #else
// only recurse and update for inner nodes: // only recurse and update for inner nodes:
@@ -245,6 +263,7 @@ void RtabmapColorOcTree::updateInnerOccupancyRecurs(RtabmapColorOcTreeNode* node
} }
node->updateOccupancyChildren(); node->updateOccupancyChildren();
node->updateColorChildren(); node->updateColorChildren();
node->updateOccupancyTypeChildren();
} }
#endif #endif
} }
@@ -1209,21 +1228,23 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
int oi=0; int oi=0;
cv::Vec2f * oPtr = obstaclesMat.ptr<cv::Vec2f>(0,0); cv::Vec2f * oPtr = obstaclesMat.ptr<cv::Vec2f>(0,0);
cv::Vec2f * gPtr = groundMat.ptr<cv::Vec2f>(0,0); cv::Vec2f * gPtr = groundMat.ptr<cv::Vec2f>(0,0);
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)
{ {
octomap::point3d pt = octree_->keyToCoord(it.getKey()); octomap::point3d pt = octree_->keyToCoord(it.getKey());
if(octree_->isNodeOccupied(*it) && it->getOccupancyType() == RtabmapColorOcTreeNode::kTypeObstacle) if(octree_->isNodeOccupied(*it) &&
it->getOccupancyType() == RtabmapColorOcTreeNode::kTypeObstacle)
{ {
// projected on ground // projected on ground
oPtr[oi][0] = pt.x(); oPtr[oi][0] = pt.x()-halfCellSize;
oPtr[oi][1] = pt.y(); oPtr[oi][1] = pt.y()-halfCellSize;
++oi; ++oi;
} }
else else
{ {
// projected on ground // projected on ground
gPtr[gi][0] = pt.x(); gPtr[gi][0] = pt.x()-halfCellSize;
gPtr[gi][1] = pt.y(); gPtr[gi][1] = pt.y()-halfCellSize;
++gi; ++gi;
} }
} }
corelib/src/util3d_mapping.cpp
+2 -2
View File
@@ -330,8 +330,8 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
{ {
//Get map size //Get map size
float margin = cellSize*10.0f; float margin = cellSize*10.0f;
xMin = minX-margin; xMin = minX-margin-cellSize/2.0f;
yMin = minY-margin; yMin = minY-margin-cellSize/2.0f;
float xMax = maxX+margin; float xMax = maxX+margin;
float yMax = maxY+margin; float yMax = maxY+margin;
if(fabs((yMax - yMin) / cellSize) > 30000 || // Max 1.5Km/1.5Km at 5 cm/cell -> 900MB if(fabs((yMax - yMin) / cellSize) > 30000 || // Max 1.5Km/1.5Km at 5 cm/cell -> 900MB