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

MapsManager: added "proj_map_frame" parameter (default false)

Browse Source
This commit is contained in:
matlabbe
2016-07-17 17:13:21 -04:00
parent d1e656bb64
commit 07f5dac624
2 changed files with 6 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MapsManager.cpp
+5 -3
View File
@@ -52,6 +52,7 @@ MapsManager::MapsManager(bool usePublicNamespace) :
projMaxObstaclesHeight_(2.0), // meters (<=0 disabled) projMaxObstaclesHeight_(2.0), // meters (<=0 disabled)
projMaxGroundHeight_(0.0), // meters (<=0 disabled, only works if proj_detect_flat_obstacles is true) projMaxGroundHeight_(0.0), // meters (<=0 disabled, only works if proj_detect_flat_obstacles is true)
projDetectFlatObstacles_(false), projDetectFlatObstacles_(false),
projMapFrame_(false),
gridCellSize_(0.05), // meters gridCellSize_(0.05), // meters
gridSize_(0), // meters gridSize_(0), // meters
gridEroded_(false), gridEroded_(false),
@@ -109,6 +110,7 @@ MapsManager::MapsManager(bool usePublicNamespace) :
} }
pnh.param("proj_max_ground_height", projMaxGroundHeight_, projMaxGroundHeight_); pnh.param("proj_max_ground_height", projMaxGroundHeight_, projMaxGroundHeight_);
pnh.param("proj_detect_flat_obstacles", projDetectFlatObstacles_, projDetectFlatObstacles_); pnh.param("proj_detect_flat_obstacles", projDetectFlatObstacles_, projDetectFlatObstacles_);
pnh.param("proj_map_frame", projMapFrame_, projMapFrame_);
// common grid map stuff // common grid map stuff
pnh.param("grid_cell_size", gridCellSize_, gridCellSize_); // m pnh.param("grid_cell_size", gridCellSize_, gridCellSize_); // m
@@ -508,7 +510,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
// add pose rotation without yaw // add pose rotation without yaw
float roll, pitch, yaw; float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw); iter->second.getEulerAngles(roll, pitch, yaw);
cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,0, roll, pitch, 0)); cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,projMapFrame_?iter->second.z():0, roll, pitch, 0));
pcl::IndicesPtr groundIndices, obstaclesIndices; pcl::IndicesPtr groundIndices, obstaclesIndices;
util3d::segmentObstaclesFromGround<pcl::PointXYZRGB>( util3d::segmentObstaclesFromGround<pcl::PointXYZRGB>(
@@ -550,7 +552,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
#ifdef RTABMAP_OCTOMAP #ifdef RTABMAP_OCTOMAP
if(updateOctomap) if(updateOctomap)
{ {
Transform tinv = Transform(0,0,0, roll, pitch, 0).inverse(); Transform tinv = Transform(0,0,projMapFrame_?iter->second.z():0, roll, pitch, 0).inverse();
groundCloud = util3d::transformPointCloud(groundCloud, tinv); groundCloud = util3d::transformPointCloud(groundCloud, tinv);
obstaclesCloud = util3d::transformPointCloud(obstaclesCloud, tinv); obstaclesCloud = util3d::transformPointCloud(obstaclesCloud, tinv);
if(octomapGroundIsObstacle_) if(octomapGroundIsObstacle_)
@@ -576,7 +578,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
// add pose rotation without yaw // add pose rotation without yaw
float roll, pitch, yaw; float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw); iter->second.getEulerAngles(roll, pitch, yaw);
cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,0, roll, pitch, 0)); cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,projMapFrame_?iter->second.z():0, roll, pitch, 0));
pcl::IndicesPtr groundIndices, obstaclesIndices; pcl::IndicesPtr groundIndices, obstaclesIndices;
util3d::segmentObstaclesFromGround<pcl::PointXYZ>( util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
src/MapsManager.h
+1
View File
@@ -79,6 +79,7 @@ private:
double projMaxObstaclesHeight_; double projMaxObstaclesHeight_;
double projMaxGroundHeight_; double projMaxGroundHeight_;
bool projDetectFlatObstacles_; bool projDetectFlatObstacles_;
bool projMapFrame_;
double gridCellSize_; double gridCellSize_;
double gridSize_; double gridSize_;
bool gridEroded_; bool gridEroded_;