i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-05 17:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Updated Info msg: changed localLoopClosureId for proximityDetectionId. point_cloud_xyzrgb nodelet: handling bgr and rgb encoding. Updated demo_find_object.launch with 0.11. MapsManager: Fixed grids generated with only one point.

Browse Source
This commit is contained in:
matlabbe
2016-03-11 20:16:52 -05:00
parent f0d91c5eac
commit f26c8b55eb
7 changed files with 75 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MapsManager.cpp
+13 -1
View File
@@ -39,6 +39,7 @@ MapsManager::MapsManager(bool usePublicNamespace) :
cloudFrustumCulling_(false),
cloudNoiseFilteringRadius_(0.0),
cloudNoiseFilteringMinNeighbors_(5),
scanDecimation_(0),
scanVoxelSize_(0.0),
scanOutputVoxelized_(false),
projMaxGroundAngle_(45.0), // degrees
@@ -48,6 +49,7 @@ MapsManager::MapsManager(bool usePublicNamespace) :
gridSize_(0), // meters
gridEroded_(false),
gridUnknownSpaceFilled_(false),
gridMaxUnknownSpaceFilledRange_(6.0),
mapFilterRadius_(0.5),
mapFilterAngle_(30.0), // degrees
mapCacheCleanup_(true)
@@ -89,6 +91,7 @@ MapsManager::MapsManager(bool usePublicNamespace) :
pnh.param("grid_size", gridSize_, gridSize_); // m
pnh.param("grid_eroded", gridEroded_, gridEroded_);
pnh.param("grid_unknown_space_filled", gridUnknownSpaceFilled_, gridUnknownSpaceFilled_);
pnh.param("grid_unknown_space_filled_max_range", gridMaxUnknownSpaceFilledRange_, gridMaxUnknownSpaceFilledRange_);
// common map stuff
pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_);
@@ -358,6 +361,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
{
scan = util3d::downsample(scan, scanDecimation_);
}
if(scanRequired || scanVoxelSize_ > 0.0)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud = util3d::laserScanToPointCloud(scan);
@@ -369,16 +373,24 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
scan = util3d::laserScan2dFromPointCloud(*scanCloud);
}
}
if(scanRequired)
{
uInsert(scans_, std::make_pair(iter->first, scanCloud));
}
}
}
if(gridRequired && scan.type() == CV_32FC2)
{
cv::Mat ground, obstacles;
util3d::occupancy2DFromLaserScan(scan, ground, obstacles, gridCellSize_, data.id() < 0 || gridUnknownSpaceFilled_, data.laserScanMaxRange());
util3d::occupancy2DFromLaserScan(
scan,
ground,
obstacles,
gridCellSize_,
data.id() < 0 || gridUnknownSpaceFilled_,
data.laserScanMaxRange()>gridMaxUnknownSpaceFilledRange_?gridMaxUnknownSpaceFilledRange_:data.laserScanMaxRange());
uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
}
}