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

Projection ray tracing, keep empty cells from projection

Browse Source
This commit is contained in:
matlabbe
2017-03-01 17:34:28 -05:00
parent 17121d5861
commit 9b3b7b09a0
4 changed files with 31 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/src/OccupancyGrid.cpp
View File

@@ -328,7 +328,6 @@ void OccupancyGrid::createLocalMap(
if(projRayTracing_)
{
cv::Mat laserScan = obstacles;
ground = cv::Mat();
obstacles = cv::Mat();
util3d::occupancy2DFromLaserScan(
laserScan,

27
corelib/src/util3d_mapping.cpp
View File

@@ -70,6 +70,23 @@ void occupancy2DFromLaserScan(
float xMin, yMin;
cv::Mat map8S = create2DMap(poses, scans, cellSize, unknownSpaceFilled, xMin, yMin, 0.0f, scanMaxRange);
// If input ground has already values, add them to map
if(ground.rows == 1 && ground.cols>0 && ground.type() == CV_32FC2)
{
for(int i=0; i<ground.cols; ++i)
{
cv::Vec2f * ptr = ground.ptr<cv::Vec2f>();
// cell
cv::Point2i cell((ptr[i][0]-xMin)/cellSize, (ptr[i][1]-yMin)/cellSize);
if(cell.x>=0 && cell.x<map8S.cols && cell.y >= 0 && cell.y < map8S.rows && map8S.at<char>(cell.y, cell.x) == -1)
{
map8S.at<char>(cell.y, cell.x) = 0;
}
}
}
// find ground cells
std::list<int> groundIndices;
for(unsigned int i=0; i< map8S.total(); ++i)
@@ -90,8 +107,9 @@ void occupancy2DFromLaserScan(
{
int y = *iter / map8S.cols;
int x = *iter - y*map8S.cols;
ground.at<cv::Vec2f>(i)[0] = (float(x))*cellSize + xMin;
ground.at<cv::Vec2f>(i)[1] = (float(y))*cellSize + yMin;
cv::Vec2f * ptr = ground.ptr<cv::Vec2f>();
ptr[i][0] = (float(x))*cellSize + xMin;
ptr[i][1] = (float(y))*cellSize + yMin;
++i;
}
}
@@ -103,8 +121,9 @@ void occupancy2DFromLaserScan(
obstacles = cv::Mat(1, (int)obstaclesCloud->size(), CV_32FC2);
for(unsigned int i=0;i<obstaclesCloud->size(); ++i)
{
obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloud->at(i).x;
obstacles.at<cv::Vec2f>(i)[1] = obstaclesCloud->at(i).y;
cv::Vec2f * ptr = obstacles.ptr<cv::Vec2f>();
ptr[i][0] = obstaclesCloud->at(i).x;
ptr[i][1] = obstaclesCloud->at(i).y;
}
}
}