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

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( void occupancy2DFromLaserScan(
const cv::Mat & scanHit, const cv::Mat & scanHitIn,
const cv::Mat & scanNoHit, const cv::Mat & scanNoHitIn,
const cv::Point3f & viewpoint, const cv::Point3f & viewpoint,
cv::Mat & empty, cv::Mat & empty,
cv::Mat & occupied, cv::Mat & occupied,
@@ -89,10 +89,41 @@ void occupancy2DFromLaserScan(
bool unknownSpaceFilled, bool unknownSpaceFilled,
float scanMaxRange) float scanMaxRange)
{ {
if(scanHit.empty() && scanNoHit.empty()) if(scanHitIn.empty() && scanNoHitIn.empty())
{ {
return; 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; std::map<int, Transform> poses;
poses.insert(std::make_pair(1, Transform::getIdentity())); poses.insert(std::make_pair(1, Transform::getIdentity()));
@@ -136,11 +167,11 @@ void occupancy2DFromLaserScan(
// copy directly obstacles precise positions // copy directly obstacles precise positions
if(scanMaxRange > cellSize) 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 else
{ {
occupied = scanHit.clone(); occupied = scanHit;
} }
} }