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

Local occupancy grid: fixed empty obstacles with scans having intensity channel when Grid/RangeMax is used.

Browse Source
This commit is contained in:
matlabbe
2020-10-21 15:38:19 -04:00
parent fbc30042c4
commit afbc0edbd6
1 changed files with 36 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
corelib/src/util3d_mapping.cpp
View File

@@ -80,8 +80,8 @@ void occupancy2DFromLaserScan(
}
void occupancy2DFromLaserScan(
const cv::Mat & scanHit,
const cv::Mat & scanNoHit,
const cv::Mat & scanHitIn,
const cv::Mat & scanNoHitIn,
const cv::Point3f & viewpoint,
cv::Mat & empty,
cv::Mat & occupied,
@@ -89,10 +89,41 @@ void occupancy2DFromLaserScan(
bool unknownSpaceFilled,
float scanMaxRange)
{
if(scanHit.empty() && scanNoHit.empty())
if(scanHitIn.empty() && scanNoHitIn.empty())
{
return;
}
cv::Mat scanHit;
cv::Mat scanNoHit;
// keep only XY channels
if(scanHitIn.channels()>2)
{
std::vector<cv::Mat> channels;
cv::split(scanHitIn,channels);
while(channels.size()>2)
{
channels.pop_back();
}
cv::merge(channels,scanHit);
}
else
{
scanHit = scanHitIn.clone(); // will be returned in occupied matrix
}
if(scanNoHitIn.channels()>2)
{
std::vector<cv::Mat> channels;
cv::split(scanNoHitIn,channels);
while(channels.size()>2)
{
channels.pop_back();
}
cv::merge(channels,scanNoHit);
}
else
{
scanNoHit = scanHitIn;
}
std::map<int, Transform> poses;
poses.insert(std::make_pair(1, Transform::getIdentity()));
@@ -136,11 +167,11 @@ void occupancy2DFromLaserScan(
// copy directly obstacles precise positions
if(scanMaxRange > cellSize)
{
occupied = util3d::rangeFiltering(LaserScan::backwardCompatibility(scanHit), 0.0f, scanMaxRange).data().clone();
occupied = util3d::rangeFiltering(LaserScan::backwardCompatibility(scanHit), 0.0f, scanMaxRange).data();
}
else
{
occupied = scanHit.clone();
occupied = scanHit;
}
}