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

OctoMap: aligned 2D projection map with OctoMap

Browse Source
This commit is contained in:
matlabbe
2019-02-12 15:36:28 -05:00
parent 5839ccbeb5
commit 3e71b3fe69
2 changed files with 4 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
corelib/src/OctoMap.cpp
View File

@@ -1063,7 +1063,6 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
} }
gridCellSize = octree_->getNodeSize(treeDepth); gridCellSize = octree_->getNodeSize(treeDepth);
float halfCellSize = gridCellSize/2.0f;
cv::Mat obstaclesMat = cv::Mat(1, (int)octree_->size(), CV_32FC2); cv::Mat obstaclesMat = cv::Mat(1, (int)octree_->size(), CV_32FC2);
cv::Mat groundMat = cv::Mat(1, (int)octree_->size(), CV_32FC2); cv::Mat groundMat = cv::Mat(1, (int)octree_->size(), CV_32FC2);
@@ -1077,15 +1076,15 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
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()-halfCellSize; oPtr[oi][0] = pt.x();
oPtr[oi][1] = pt.y()-halfCellSize; oPtr[oi][1] = pt.y();
++oi; ++oi;
} }
else else
{ {
// projected on ground // projected on ground
gPtr[gi][0] = pt.x()-halfCellSize; gPtr[gi][0] = pt.x();
gPtr[gi][1] = pt.y()-halfCellSize; gPtr[gi][1] = pt.y();
++gi; ++gi;
} }
} }

2
corelib/src/util3d_mapping.cpp
View File

@@ -300,9 +300,7 @@ 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;
xMin -= cellSize/2.0f;
yMin = minY-margin; yMin = minY-margin;
yMin += 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