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

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

10
corelib/include/rtabmap/core/impl/util3d_mapping.hpp
View File

@@ -255,8 +255,9 @@ void occupancy2DFromGroundObstacles(
ground = cv::Mat(1, (int)groundCloudProjected->size(), CV_32FC2); ground = cv::Mat(1, (int)groundCloudProjected->size(), CV_32FC2);
for(unsigned int i=0;i<groundCloudProjected->size(); ++i) for(unsigned int i=0;i<groundCloudProjected->size(); ++i)
{ {
ground.at<cv::Vec2f>(i)[0] = groundCloudProjected->at(i).x; cv::Vec2f * ptr = ground.ptr<cv::Vec2f>();
ground.at<cv::Vec2f>(i)[1] = groundCloudProjected->at(i).y; ptr[i][0] = groundCloudProjected->at(i).x;
ptr[i][1] = groundCloudProjected->at(i).y;
} }
} }
@@ -272,8 +273,9 @@ void occupancy2DFromGroundObstacles(
obstacles = cv::Mat(1, (int)obstaclesCloudProjected->size(), CV_32FC2); obstacles = cv::Mat(1, (int)obstaclesCloudProjected->size(), CV_32FC2);
for(unsigned int i=0;i<obstaclesCloudProjected->size(); ++i) for(unsigned int i=0;i<obstaclesCloudProjected->size(); ++i)
{ {
obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloudProjected->at(i).x; cv::Vec2f * ptr = obstacles.ptr<cv::Vec2f>();
obstacles.at<cv::Vec2f>(i)[1] = obstaclesCloudProjected->at(i).y; ptr[i][0] = obstaclesCloudProjected->at(i).x;
ptr[i][1] = obstaclesCloudProjected->at(i).y;
} }
} }
} }

1
corelib/src/OccupancyGrid.cpp
View File

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

27
corelib/src/util3d_mapping.cpp
View File

@@ -70,6 +70,23 @@ void occupancy2DFromLaserScan(
float xMin, yMin; float xMin, yMin;
cv::Mat map8S = create2DMap(poses, scans, cellSize, unknownSpaceFilled, xMin, yMin, 0.0f, scanMaxRange); 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 // find ground cells
std::list<int> groundIndices; std::list<int> groundIndices;
for(unsigned int i=0; i< map8S.total(); ++i) for(unsigned int i=0; i< map8S.total(); ++i)
@@ -90,8 +107,9 @@ void occupancy2DFromLaserScan(
{ {
int y = *iter / map8S.cols; int y = *iter / map8S.cols;
int x = *iter - y*map8S.cols; int x = *iter - y*map8S.cols;
ground.at<cv::Vec2f>(i)[0] = (float(x))*cellSize + xMin; cv::Vec2f * ptr = ground.ptr<cv::Vec2f>();
ground.at<cv::Vec2f>(i)[1] = (float(y))*cellSize + yMin; ptr[i][0] = (float(x))*cellSize + xMin;
ptr[i][1] = (float(y))*cellSize + yMin;
++i; ++i;
} }
} }
@@ -103,8 +121,9 @@ void occupancy2DFromLaserScan(
obstacles = cv::Mat(1, (int)obstaclesCloud->size(), CV_32FC2); obstacles = cv::Mat(1, (int)obstaclesCloud->size(), CV_32FC2);
for(unsigned int i=0;i<obstaclesCloud->size(); ++i) for(unsigned int i=0;i<obstaclesCloud->size(); ++i)
{ {
obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloud->at(i).x; cv::Vec2f * ptr = obstacles.ptr<cv::Vec2f>();
obstacles.at<cv::Vec2f>(i)[1] = obstaclesCloud->at(i).y; ptr[i][0] = obstaclesCloud->at(i).x;
ptr[i][1] = obstaclesCloud->at(i).y;
} }
} }
} }

4
guilib/src/ui/preferencesDialog.ui
View File

@@ -63,7 +63,7 @@
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>
<y>-491</y> <y>-483</y>
<width>673</width> <width>673</width>
<height>2649</height> <height>2649</height>
</rect> </rect>
@@ -8860,7 +8860,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<item row="4" column="1"> <item row="4" column="1">
<widget class="QLabel" name="label_363"> <widget class="QLabel" name="label_363">
<property name="text"> <property name="text">
<string>If 3D is not checked below, 2D ray tracing is done for each projected obstacle, filling unknown space between the sensor and obstacles.</string> <string>2D ray tracing is done for each projected obstacle (when 3D is not checked below), filling unknown space between the sensor and obstacles.</string>
</property> </property>
<property name="wordWrap"> <property name="wordWrap">
<bool>true</bool> <bool>true</bool>