00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "MapsManager.h"
00029
00030 #include <rtabmap/utilite/ULogger.h>
00031 #include <rtabmap/utilite/UTimer.h>
00032 #include <rtabmap/utilite/UStl.h>
00033 #include <rtabmap/utilite/UConversion.h>
00034 #include <rtabmap/core/util3d_mapping.h>
00035 #include <rtabmap/core/util3d_filtering.h>
00036 #include <rtabmap/core/util3d_transforms.h>
00037 #include <rtabmap/core/util2d.h>
00038 #include <rtabmap/core/Memory.h>
00039 #include <rtabmap/core/Graph.h>
00040 #include <rtabmap/core/Version.h>
00041
00042 #include <nav_msgs/OccupancyGrid.h>
00043 #include <ros/ros.h>
00044
00045 #include <pcl_conversions/pcl_conversions.h>
00046
00047 #ifdef WITH_OCTOMAP_ROS
00048 #ifdef RTABMAP_OCTOMAP
00049 #include <octomap_msgs/conversions.h>
00050 #include <rtabmap/core/OctoMap.h>
00051 #endif
00052 #endif
00053
00054 using namespace rtabmap;
00055
00056 MapsManager::MapsManager(bool usePublicNamespace) :
00057 cloudDecimation_(4),
00058 cloudMaxDepth_(4.0),
00059 cloudMinDepth_(0.0),
00060 cloudVoxelSize_(0.05),
00061 cloudFloorCullingHeight_(0.0),
00062 cloudCeilingCullingHeight_(0.0),
00063 cloudOutputVoxelized_(false),
00064 cloudFrustumCulling_(false),
00065 cloudNoiseFilteringRadius_(0.0),
00066 cloudNoiseFilteringMinNeighbors_(5),
00067 scanDecimation_(0),
00068 scanVoxelSize_(0.0),
00069 scanOutputVoxelized_(false),
00070 projMaxGroundAngle_(45.0),
00071 projMinClusterSize_(20),
00072 projMaxObstaclesHeight_(2.0),
00073 projMaxGroundHeight_(0.0),
00074 projDetectFlatObstacles_(false),
00075 projMapFrame_(false),
00076 gridCellSize_(0.05),
00077 gridSize_(0),
00078 gridEroded_(false),
00079 gridUnknownSpaceFilled_(false),
00080 gridMaxUnknownSpaceFilledRange_(6.0),
00081 mapFilterRadius_(0.0),
00082 mapFilterAngle_(30.0),
00083 mapCacheCleanup_(true),
00084 negativePosesIgnored_(false),
00085 octomap_(0),
00086 octomapTreeDepth_(16),
00087 octomapGroundIsObstacle_(false)
00088 {
00089
00090 ros::NodeHandle nh;
00091 ros::NodeHandle pnh("~");
00092
00093
00094 pnh.param("cloud_decimation", cloudDecimation_, cloudDecimation_);
00095 pnh.param("cloud_max_depth", cloudMaxDepth_, cloudMaxDepth_);
00096 pnh.param("cloud_min_depth", cloudMinDepth_, cloudMinDepth_);
00097 pnh.param("cloud_voxel_size", cloudVoxelSize_, cloudVoxelSize_);
00098 pnh.param("cloud_floor_culling_height", cloudFloorCullingHeight_, cloudFloorCullingHeight_);
00099 pnh.param("cloud_ceiling_culling_height", cloudCeilingCullingHeight_, cloudCeilingCullingHeight_);
00100 if(cloudFloorCullingHeight_ > 0 &&
00101 cloudCeilingCullingHeight_ > 0 &&
00102 cloudCeilingCullingHeight_ < cloudFloorCullingHeight_)
00103 {
00104 ROS_WARN("\"cloud_floor_culling_height\" should be lower than \"cloud_ceiling_culling_height\", setting \"cloud_ceiling_culling_height\" to 0 (disabled).");
00105 cloudCeilingCullingHeight_ = 0;
00106 }
00107 pnh.param("cloud_output_voxelized", cloudOutputVoxelized_, cloudOutputVoxelized_);
00108 pnh.param("cloud_frustum_culling", cloudFrustumCulling_, cloudFrustumCulling_);
00109 pnh.param("cloud_noise_filtering_radius", cloudNoiseFilteringRadius_, cloudNoiseFilteringRadius_);
00110 pnh.param("cloud_noise_filtering_min_neighbors", cloudNoiseFilteringMinNeighbors_, cloudNoiseFilteringMinNeighbors_);
00111
00112
00113 pnh.param("scan_decimation", scanDecimation_, scanDecimation_);
00114 pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
00115 pnh.param("scan_output_voxelized", scanOutputVoxelized_, scanOutputVoxelized_);
00116
00117
00118 pnh.param("proj_max_ground_angle", projMaxGroundAngle_, projMaxGroundAngle_);
00119 pnh.param("proj_min_cluster_size", projMinClusterSize_, projMinClusterSize_);
00120 if(pnh.hasParam("proj_max_height") && !pnh.hasParam("proj_max_obstacles_height"))
00121 {
00122 ROS_WARN("Parameter \"proj_max_height\" has been renamed "
00123 "to \"proj_max_obstacles_height\"! Your value is still copied to "
00124 "corresponding parameter.");
00125 pnh.param("proj_max_height", projMaxObstaclesHeight_, projMaxObstaclesHeight_);
00126 }
00127 else
00128 {
00129 pnh.param("proj_max_obstacles_height", projMaxObstaclesHeight_, projMaxObstaclesHeight_);
00130 }
00131 pnh.param("proj_max_ground_height", projMaxGroundHeight_, projMaxGroundHeight_);
00132 pnh.param("proj_detect_flat_obstacles", projDetectFlatObstacles_, projDetectFlatObstacles_);
00133 pnh.param("proj_map_frame", projMapFrame_, projMapFrame_);
00134
00135
00136 pnh.param("grid_cell_size", gridCellSize_, gridCellSize_);
00137 if(gridCellSize_ <= 0)
00138 {
00139 ROS_FATAL("\"grid_cell_size\" (%f) should be greater than 0!", gridCellSize_);
00140 }
00141 pnh.param("grid_size", gridSize_, gridSize_);
00142 pnh.param("grid_eroded", gridEroded_, gridEroded_);
00143 pnh.param("grid_unknown_space_filled", gridUnknownSpaceFilled_, gridUnknownSpaceFilled_);
00144 pnh.param("grid_unknown_space_filled_max_range", gridMaxUnknownSpaceFilledRange_, gridMaxUnknownSpaceFilledRange_);
00145
00146
00147 pnh.param("map_filter_radius", mapFilterRadius_, mapFilterRadius_);
00148 pnh.param("map_filter_angle", mapFilterAngle_, mapFilterAngle_);
00149 pnh.param("map_cleanup", mapCacheCleanup_, mapCacheCleanup_);
00150 pnh.param("map_negative_poses_ignored", negativePosesIgnored_, negativePosesIgnored_);
00151
00152 #ifdef WITH_OCTOMAP_ROS
00153 #ifdef RTABMAP_OCTOMAP
00154 octomap_ = new OctoMap(gridCellSize_);
00155 pnh.param("octomap_tree_depth", octomapTreeDepth_, octomapTreeDepth_);
00156 if(octomapTreeDepth_ > 16)
00157 {
00158 ROS_WARN("octomap_tree_depth maximum is 16");
00159 octomapTreeDepth_ = 16;
00160 }
00161 else if(octomapTreeDepth_ < 0)
00162 {
00163 ROS_WARN("octomap_tree_depth cannot be negative, set to 16 instead");
00164 octomapTreeDepth_ = 16;
00165 }
00166 pnh.param("octomap_ground_is_obstacle", octomapGroundIsObstacle_, octomapGroundIsObstacle_);
00167 #endif
00168 #endif
00169
00170
00171
00172
00173 bool latch = true;
00174 pnh.param("latch", latch, latch);
00175
00176
00177 if(usePublicNamespace)
00178 {
00179 cloudMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud_map", 1, latch);
00180 projMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("proj_map", 1, latch);
00181 gridMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1, latch);
00182 scanMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("scan_map", 1, latch);
00183 #ifdef WITH_OCTOMAP_ROS
00184 #ifdef RTABMAP_OCTOMAP
00185 octoMapPubBin_ = nh.advertise<octomap_msgs::Octomap>("octomap_binary", 1, latch);
00186 octoMapPubFull_ = nh.advertise<octomap_msgs::Octomap>("octomap_full", 1, latch);
00187 octoMapCloud_ = nh.advertise<sensor_msgs::PointCloud2>("octomap_cloud", 1, latch);
00188 octoMapEmptySpace_ = nh.advertise<sensor_msgs::PointCloud2>("octomap_empty_space", 1, latch);
00189 octoMapProj_ = nh.advertise<nav_msgs::OccupancyGrid>("octomap_proj", 1, latch);
00190 #endif
00191 #endif
00192 }
00193 else
00194 {
00195 cloudMapPub_ = pnh.advertise<sensor_msgs::PointCloud2>("cloud_map", 1, latch);
00196 projMapPub_ = pnh.advertise<nav_msgs::OccupancyGrid>("proj_map", 1, latch);
00197 gridMapPub_ = pnh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1, latch);
00198 scanMapPub_ = pnh.advertise<sensor_msgs::PointCloud2>("scan_map", 1, latch);
00199 #ifdef WITH_OCTOMAP_ROS
00200 #ifdef RTABMAP_OCTOMAP
00201 octoMapPubBin_ = pnh.advertise<octomap_msgs::Octomap>("octomap_binary", 1, latch);
00202 octoMapPubFull_ = pnh.advertise<octomap_msgs::Octomap>("octomap_full", 1, latch);
00203 octoMapCloud_ = pnh.advertise<sensor_msgs::PointCloud2>("octomap_cloud", 1, latch);
00204 octoMapEmptySpace_ = pnh.advertise<sensor_msgs::PointCloud2>("octomap_cloud_ground", 1, latch);
00205 octoMapProj_ = pnh.advertise<nav_msgs::OccupancyGrid>("octomap_proj", 1, latch);
00206 #endif
00207 #endif
00208 }
00209 }
00210
00211 MapsManager::~MapsManager() {
00212 clear();
00213
00214 #ifdef WITH_OCTOMAP_ROS
00215 #ifdef RTABMAP_OCTOMAP
00216 if(octomap_)
00217 {
00218 delete octomap_;
00219 octomap_ = 0;
00220 }
00221 #endif
00222 #endif
00223 }
00224
00225 void MapsManager::clear()
00226 {
00227 clouds_.clear();
00228 cameraModels_.clear();
00229 projMaps_.clear();
00230 gridMaps_.clear();
00231 #ifdef WITH_OCTOMAP_ROS
00232 #ifdef RTABMAP_OCTOMAP
00233 octomap_->clear();
00234 #endif
00235 #endif
00236 }
00237
00238 bool MapsManager::hasSubscribers() const
00239 {
00240 return cloudMapPub_.getNumSubscribers() != 0 ||
00241 projMapPub_.getNumSubscribers() != 0 ||
00242 gridMapPub_.getNumSubscribers() != 0 ||
00243 scanMapPub_.getNumSubscribers() != 0 ||
00244 octoMapPubBin_.getNumSubscribers() != 0 ||
00245 octoMapPubFull_.getNumSubscribers() != 0 ||
00246 octoMapCloud_.getNumSubscribers() != 0 ||
00247 octoMapEmptySpace_.getNumSubscribers() != 0 ||
00248 octoMapProj_.getNumSubscribers() != 0;
00249 }
00250
00251 std::map<int, Transform> MapsManager::getFilteredPoses(const std::map<int, Transform> & poses)
00252 {
00253 if(mapFilterRadius_ > 0.0)
00254 {
00255
00256 double angle = mapFilterAngle_ == 0.0?CV_PI+0.1:mapFilterAngle_*CV_PI/180.0;
00257 return rtabmap::graph::radiusPosesFiltering(poses, mapFilterRadius_, angle);
00258 }
00259 return std::map<int, Transform>();
00260 }
00261
00262 std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
00263 const std::map<int, rtabmap::Transform> & poses,
00264 const rtabmap::Memory * memory,
00265 bool updateCloud,
00266 bool updateProj,
00267 bool updateGrid,
00268 bool updateScan,
00269 bool updateOctomap,
00270 const std::map<int, rtabmap::Signature> & signatures)
00271 {
00272 if(!updateCloud && !updateProj && !updateGrid && !updateScan && !updateOctomap)
00273 {
00274
00275 updateCloud = cloudMapPub_.getNumSubscribers() != 0;
00276 updateProj = projMapPub_.getNumSubscribers() != 0;
00277 updateGrid = gridMapPub_.getNumSubscribers() != 0;
00278 updateScan = scanMapPub_.getNumSubscribers() != 0;
00279 updateOctomap =
00280 octoMapPubBin_.getNumSubscribers() != 0 ||
00281 octoMapPubFull_.getNumSubscribers() != 0 ||
00282 octoMapCloud_.getNumSubscribers() != 0 ||
00283 octoMapEmptySpace_.getNumSubscribers() != 0 ||
00284 octoMapProj_.getNumSubscribers() != 0;
00285 }
00286
00287 #ifndef WITH_OCTOMAP_ROS
00288 updateOctomap = false;
00289 #endif
00290 #ifndef RTABMAP_OCTOMAP
00291 updateOctomap = false;
00292 #endif
00293
00294
00295 UDEBUG("Updating map caches...");
00296
00297 if(!memory && signatures.size() == 0)
00298 {
00299 ROS_ERROR("Memory and signatures should not be both null!?");
00300 return std::map<int, rtabmap::Transform>();
00301 }
00302
00303 std::map<int, rtabmap::Transform> filteredPoses;
00304
00305
00306 if(updateCloud || updateProj || updateGrid || updateScan || updateOctomap)
00307 {
00308
00309 if(mapFilterRadius_ > 0.0)
00310 {
00311 UDEBUG("Filter nodes...");
00312 double angle = mapFilterAngle_ == 0.0?CV_PI+0.1:mapFilterAngle_*CV_PI/180.0;
00313 filteredPoses = rtabmap::graph::radiusPosesFiltering(poses, mapFilterRadius_, angle);
00314 for(std::map<int, rtabmap::Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
00315 {
00316 if(iter->first <=0)
00317 {
00318
00319 filteredPoses.insert(*iter);
00320 }
00321 else
00322 {
00323 break;
00324 }
00325 }
00326 }
00327 else
00328 {
00329 filteredPoses = poses;
00330 }
00331
00332 if(negativePosesIgnored_)
00333 {
00334 for(std::map<int, rtabmap::Transform>::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end();)
00335 {
00336 if(iter->first <= 0)
00337 {
00338 filteredPoses.erase(iter++);
00339 }
00340 else
00341 {
00342 ++iter;
00343 }
00344 }
00345 }
00346
00347 bool longUpdate = false;
00348 if(filteredPoses.size() > 20)
00349 {
00350 if(updateCloud && clouds_.size() < 5)
00351 {
00352 ROS_WARN("Many clouds should be created (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-clouds_.size()));
00353 longUpdate = true;
00354 }
00355 else if(updateProj && projMaps_.size() < 5)
00356 {
00357 ROS_WARN("Many occupancy grid map from projections should be created (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-projMaps_.size()));
00358 longUpdate = true;
00359 }
00360 else if(updateGrid && gridMaps_.size() < 5)
00361 {
00362 ROS_WARN("Many occupancy grid map from laser scans should be created (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-gridMaps_.size()));
00363 longUpdate = true;
00364 }
00365 else if(updateScan && scans_.size() < 5)
00366 {
00367 ROS_WARN("Many scans should be created (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-scans_.size()));
00368 longUpdate = true;
00369 }
00370 #ifdef WITH_OCTOMAP_ROS
00371 #ifdef RTABMAP_OCTOMAP
00372 else if(updateOctomap && octomap_->addedNodes().size() < 5)
00373 {
00374 ROS_WARN("Many clouds should be added to octomap (~%d), this may take a while to update the map(s)...", int(filteredPoses.size()-octomap_->addedNodes().size()));
00375 longUpdate = true;
00376 }
00377 #endif
00378 #endif
00379 }
00380
00381 for(std::map<int, rtabmap::Transform>::iterator iter=filteredPoses.begin(); iter!=filteredPoses.end(); ++iter)
00382 {
00383 if(!iter->second.isNull())
00384 {
00385 rtabmap::SensorData data;
00386 bool rgbDepthRequired = updateCloud && (iter->first < 0 || !uContains(clouds_, iter->first));
00387 bool depthRequired = updateProj && (iter->first < 0 || !uContains(projMaps_, iter->first));
00388 bool gridRequired = updateGrid && (iter->first < 0 || !uContains(gridMaps_, iter->first));
00389 bool scanRequired = updateScan && (iter->first < 0 || !uContains(scans_, iter->first));
00390
00391 #ifdef WITH_OCTOMAP_ROS
00392 #ifdef RTABMAP_OCTOMAP
00393 if(!rgbDepthRequired)
00394 {
00395 rgbDepthRequired = updateOctomap &&
00396 (iter->first < 0 ||
00397 octomap_->addedNodes().empty() ||
00398 iter->first > octomap_->addedNodes().rbegin()->first);
00399 }
00400 #endif
00401 #endif
00402
00403 if(rgbDepthRequired ||
00404 depthRequired ||
00405 scanRequired ||
00406 gridRequired)
00407 {
00408 UDEBUG("Data required for %d", iter->first);
00409 std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first);
00410 if(findIter != signatures.end())
00411 {
00412 data = findIter->second.sensorData();
00413 }
00414 else if(memory)
00415 {
00416 data = memory->getSignatureDataConst(iter->first);
00417 }
00418 }
00419
00420 if(data.id() != 0)
00421 {
00422 if(!(data.imageCompressed().empty() && data.imageRaw().empty()) &&
00423 !(data.depthOrRightCompressed().empty() && data.depthOrRightRaw().empty()) &&
00424 (data.cameraModels().size() || data.stereoCameraModel().isValidForProjection()))
00425 {
00426
00427 cv::Mat image, depth, scan;
00428 data.uncompressData(
00429 (rgbDepthRequired||data.stereoCameraModel().isValidForProjection()) ? &image:0,
00430 (rgbDepthRequired||depthRequired) ? &depth:0,
00431 scanRequired||gridRequired?&scan:0);
00432
00433 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB;
00434 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudXYZ;
00435 if(rgbDepthRequired)
00436 {
00437 UDEBUG("rgbDepthRequired");
00438 if(!image.empty() && !depth.empty())
00439 {
00440 pcl::IndicesPtr validIndices(new std::vector<int>);
00441 cloudRGB = util3d::cloudRGBFromSensorData(
00442 data,
00443 cloudDecimation_,
00444 cloudMaxDepth_,
00445 cloudMinDepth_,
00446 validIndices.get());
00447 if(cloudVoxelSize_)
00448 {
00449 cloudRGB = util3d::voxelize(cloudRGB, validIndices, cloudVoxelSize_);
00450 }
00451 if(cloudRGB->size() && cloudNoiseFilteringRadius_ > 0.0 && cloudNoiseFilteringMinNeighbors_ > 0)
00452 {
00453 pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloudRGB, cloudNoiseFilteringRadius_, cloudNoiseFilteringMinNeighbors_);
00454 pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
00455 pcl::copyPointCloud(*cloudRGB, *indices, *tmp);
00456 cloudRGB = tmp;
00457 }
00458 }
00459 else
00460 {
00461 ROS_ERROR("RGB or Depth image not found (node=%d)!", iter->first);
00462 }
00463 }
00464 else if(depthRequired)
00465 {
00466 UDEBUG("depthRequired");
00467 if( !depth.empty())
00468 {
00469 pcl::IndicesPtr validIndices(new std::vector<int>);
00470 cloudXYZ = util3d::cloudFromSensorData(
00471 data,
00472 cloudDecimation_,
00473 cloudMaxDepth_,
00474 cloudMinDepth_,
00475 validIndices.get());
00476 UASSERT(gridCellSize_ > 0);
00477 cloudXYZ = util3d::voxelize(cloudXYZ, validIndices, gridCellSize_);
00478 if(cloudXYZ->size() && cloudNoiseFilteringRadius_ > 0.0 && cloudNoiseFilteringMinNeighbors_ > 0)
00479 {
00480 pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloudXYZ, cloudNoiseFilteringRadius_, cloudNoiseFilteringMinNeighbors_);
00481 pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
00482 pcl::copyPointCloud(*cloudXYZ, *indices, *tmp);
00483 cloudXYZ = tmp;
00484 }
00485 }
00486 else
00487 {
00488 ROS_ERROR("RGB or Depth image not found (node=%d)!", iter->first);
00489 }
00490 }
00491
00492 if(cloudRGB.get())
00493 {
00494 uInsert(clouds_, std::make_pair(iter->first, cloudRGB));
00495
00496
00497
00498 std::vector<rtabmap::CameraModel> models;
00499 if(data.stereoCameraModel().isValidForProjection())
00500 {
00501
00502 rtabmap::CameraModel model = data.stereoCameraModel().left();
00503 model.setImageSize(cv::Size(data.imageRaw().cols, data.imageRaw().rows));
00504 models.push_back(model);
00505 }
00506 else if(data.cameraModels().size())
00507 {
00508 UASSERT_MSG(data.imageRaw().cols % data.cameraModels().size() == 0,
00509 uFormat("data.imageRaw().cols=%d data.cameraModels().size()=%d",
00510 data.imageRaw().cols, (int)data.cameraModels().size()).c_str());
00511
00512 models.resize(data.cameraModels().size());
00513 for(unsigned int i=0; i<data.cameraModels().size(); ++i)
00514 {
00515 models[i] = data.cameraModels()[i];
00516 models[i].setImageSize(cv::Size(data.imageRaw().cols/data.cameraModels().size(), data.imageRaw().rows));
00517 }
00518 }
00519 uInsert(cameraModels_, std::make_pair(iter->first, models));
00520 }
00521
00522 if(depthRequired || updateOctomap)
00523 {
00524 UDEBUG("Creating proj map / octomap for %d...", iter->first);
00525 cv::Mat ground, obstacles;
00526 if(cloudRGB.get())
00527 {
00528 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudClipped = cloudRGB;
00529 if(cloudClipped->size() && projMaxObstaclesHeight_ > 0)
00530 {
00531 cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits<int>::min(), projMaxObstaclesHeight_);
00532 }
00533 if(cloudClipped->size() && gridCellSize_ > cloudVoxelSize_)
00534 {
00535 cloudClipped = util3d::voxelize(cloudClipped, gridCellSize_);
00536 }
00537 if(cloudClipped->size())
00538 {
00539
00540 float roll, pitch, yaw;
00541 iter->second.getEulerAngles(roll, pitch, yaw);
00542 cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,projMapFrame_?iter->second.z():0, roll, pitch, 0));
00543
00544 pcl::IndicesPtr groundIndices, obstaclesIndices;
00545 util3d::segmentObstaclesFromGround<pcl::PointXYZRGB>(
00546 cloudClipped,
00547 groundIndices,
00548 obstaclesIndices,
00549 20,
00550 projMaxGroundAngle_*M_PI/180.0,
00551 gridCellSize_*2.0f,
00552 projMinClusterSize_,
00553 projDetectFlatObstacles_,
00554 projMaxGroundHeight_);
00555
00556 pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
00557 pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
00558
00559 if(groundIndices->size())
00560 {
00561 pcl::copyPointCloud(*cloudClipped, *groundIndices, *groundCloud);
00562 }
00563
00564 if(obstaclesIndices->size())
00565 {
00566 pcl::copyPointCloud(*cloudClipped, *obstaclesIndices, *obstaclesCloud);
00567 }
00568
00569 if(updateProj)
00570 {
00571 util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(
00572 groundCloud,
00573 obstaclesCloud,
00574 ground,
00575 obstacles,
00576 gridCellSize_);
00577 uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
00578 }
00579
00580 #ifdef WITH_OCTOMAP_ROS
00581 #ifdef RTABMAP_OCTOMAP
00582 if(updateOctomap)
00583 {
00584 Transform tinv = Transform(0,0,projMapFrame_?iter->second.z():0, roll, pitch, 0).inverse();
00585 groundCloud = util3d::transformPointCloud(groundCloud, tinv);
00586 obstaclesCloud = util3d::transformPointCloud(obstaclesCloud, tinv);
00587 if(octomapGroundIsObstacle_)
00588 {
00589 *obstaclesCloud += *groundCloud;
00590 groundCloud->clear();
00591 }
00592 octomap_->addToCache(iter->first, groundCloud, obstaclesCloud);
00593 }
00594 #endif
00595 #endif
00596 }
00597 }
00598 else if(updateProj && cloudXYZ.get())
00599 {
00600 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudClipped = cloudXYZ;
00601 if(cloudClipped->size() && projMaxObstaclesHeight_ > 0)
00602 {
00603 cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits<int>::min(), projMaxObstaclesHeight_);
00604 }
00605 if(cloudClipped->size())
00606 {
00607
00608 float roll, pitch, yaw;
00609 iter->second.getEulerAngles(roll, pitch, yaw);
00610 cloudClipped = util3d::transformPointCloud(cloudClipped, Transform(0,0,projMapFrame_?iter->second.z():0, roll, pitch, 0));
00611
00612 pcl::IndicesPtr groundIndices, obstaclesIndices;
00613 util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
00614 cloudClipped,
00615 groundIndices,
00616 obstaclesIndices,
00617 20,
00618 projMaxGroundAngle_*M_PI/180.0,
00619 gridCellSize_*2.0f,
00620 projMinClusterSize_,
00621 projDetectFlatObstacles_,
00622 projMaxGroundHeight_);
00623
00624 util3d::occupancy2DFromGroundObstacles<pcl::PointXYZ>(
00625 cloudClipped,
00626 groundIndices,
00627 obstaclesIndices,
00628 ground,
00629 obstacles,
00630 gridCellSize_);
00631 uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
00632 }
00633 }
00634 }
00635
00636 if(scanRequired || gridRequired)
00637 {
00638 if(scan.cols && (scanRequired || scanVoxelSize_ > 0.0 || scanDecimation_ > 1))
00639 {
00640 if(scanDecimation_ > 1)
00641 {
00642 scan = util3d::downsample(scan, scanDecimation_);
00643 }
00644
00645 if(scanRequired || scanVoxelSize_ > 0.0)
00646 {
00647 pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud = util3d::laserScanToPointCloud(scan);
00648 if(scanVoxelSize_ > 0.0)
00649 {
00650 scanCloud = util3d::voxelize(scanCloud, scanVoxelSize_);
00651 if(gridRequired && scan.type() == CV_32FC2)
00652 {
00653 scan = util3d::laserScan2dFromPointCloud(*scanCloud);
00654 }
00655 }
00656
00657 if(scanRequired)
00658 {
00659 uInsert(scans_, std::make_pair(iter->first, scanCloud));
00660 }
00661 }
00662 }
00663
00664 if(gridRequired && scan.type() == CV_32FC2)
00665 {
00666 cv::Mat ground, obstacles;
00667 util3d::occupancy2DFromLaserScan(
00668 scan,
00669 ground,
00670 obstacles,
00671 gridCellSize_,
00672 data.id() < 0 || gridUnknownSpaceFilled_,
00673 data.laserScanMaxRange()>gridMaxUnknownSpaceFilledRange_?gridMaxUnknownSpaceFilledRange_:data.laserScanMaxRange());
00674 uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
00675 }
00676 }
00677 }
00678 else
00679 {
00680 ROS_ERROR("Some data missing for node %d to update the maps (image=%d, depth=%d, camera=%d)",
00681 iter->first,
00682 !(data.imageCompressed().empty() && data.imageRaw().empty())?1:0,
00683 !(data.depthOrRightCompressed().empty() && data.depthOrRightRaw().empty())?1:0,
00684 (data.cameraModels().size() || data.stereoCameraModel().isValidForProjection())?1:0);
00685 }
00686 }
00687 }
00688 else
00689 {
00690 ROS_ERROR("Pose null for node %d", iter->first);
00691 }
00692 }
00693
00694 #ifdef WITH_OCTOMAP_ROS
00695 #ifdef RTABMAP_OCTOMAP
00696 if(updateOctomap)
00697 {
00698 UTimer time;
00699 octomap_->update(filteredPoses);
00700 ROS_INFO("Octomap update time = %fs", time.ticks());
00701 }
00702 #endif
00703 #endif
00704
00705
00706 UDEBUG("Cleanup not used nodes");
00707 for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=clouds_.begin();
00708 iter!=clouds_.end();)
00709 {
00710 if(!uContains(poses, iter->first))
00711 {
00712 clouds_.erase(iter++);
00713 }
00714 else
00715 {
00716 ++iter;
00717 }
00718 }
00719 for(std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter=scans_.begin();
00720 iter!=scans_.end();)
00721 {
00722 if(!uContains(poses, iter->first))
00723 {
00724 scans_.erase(iter++);
00725 }
00726 else
00727 {
00728 ++iter;
00729 }
00730 }
00731 for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter=projMaps_.begin();
00732 iter!=projMaps_.end();)
00733 {
00734 if(!uContains(poses, iter->first))
00735 {
00736 projMaps_.erase(iter++);
00737 }
00738 else
00739 {
00740 ++iter;
00741 }
00742 }
00743 for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter=gridMaps_.begin();
00744 iter!=gridMaps_.end();)
00745 {
00746 if(!uContains(poses, iter->first))
00747 {
00748 gridMaps_.erase(iter++);
00749 }
00750 else
00751 {
00752 ++iter;
00753 }
00754 }
00755 for(std::map<int, std::vector<rtabmap::CameraModel> >::iterator iter=cameraModels_.begin();
00756 iter!=cameraModels_.end();)
00757 {
00758 if(!uContains(poses, iter->first))
00759 {
00760 cameraModels_.erase(iter++);
00761 }
00762 else
00763 {
00764 ++iter;
00765 }
00766 }
00767
00768 if(longUpdate)
00769 {
00770 ROS_WARN("Map(s) updated!");
00771 }
00772 }
00773
00774 return filteredPoses;
00775 }
00776
00777 void MapsManager::publishMaps(
00778 const std::map<int, rtabmap::Transform> & poses,
00779 const ros::Time & stamp,
00780 const std::string & mapFrameId)
00781 {
00782 UDEBUG("Publishing maps...");
00783
00784
00785 if(cloudMapPub_.getNumSubscribers())
00786 {
00787
00788 UTimer time;
00789 pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
00790 int count = 0;
00791 std::list<std::pair<int, Transform> > negativePoses;
00792 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
00793 {
00794 if(iter->first > 0)
00795 {
00796 std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator jter = clouds_.find(iter->first);
00797 if(jter != clouds_.end())
00798 {
00799 pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
00800 *assembledCloud+=*transformed;
00801 ++count;
00802 }
00803 }
00804 else
00805 {
00806 negativePoses.push_back(*iter);
00807 }
00808 }
00809
00810 for(std::list<std::pair<int, Transform> >::reverse_iterator iter=negativePoses.rbegin(); iter!=negativePoses.rend(); ++iter)
00811 {
00812 std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator jter = clouds_.find(iter->first);
00813
00814 if(jter != clouds_.end() && jter->second->size())
00815 {
00816 std::map<int, std::vector<CameraModel> >::iterator kter = cameraModels_.find(iter->first);
00817 if(cloudFrustumCulling_ && kter != cameraModels_.end() && assembledCloud->size())
00818 {
00819 for(unsigned int i=0; i<kter->second.size(); ++i)
00820 {
00821 if(kter->second[i].isValidForProjection())
00822 {
00823 assembledCloud = util3d::frustumFiltering(
00824 assembledCloud,
00825 iter->second,
00826 kter->second[i].horizontalFOV(),
00827 kter->second[i].verticalFOV(),
00828 0.0f,
00829 cloudMaxDepth_>0.0?cloudMaxDepth_:999999.,
00830 true);
00831
00832
00833 pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
00834 *assembledCloud+=*transformed;
00835 ++count;
00836 }
00837 }
00838 }
00839 else
00840 {
00841 pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
00842 if(assembledCloud->size())
00843 {
00844 *assembledCloud+=*transformed;
00845 }
00846 else
00847 {
00848 assembledCloud = transformed;
00849 }
00850 ++count;
00851 }
00852 }
00853 }
00854
00855 if(assembledCloud->size() && (cloudFloorCullingHeight_ > 0.0 || cloudCeilingCullingHeight_ > 0.0))
00856 {
00857 assembledCloud = util3d::passThrough(assembledCloud, "z",
00858 cloudFloorCullingHeight_>0.0?cloudFloorCullingHeight_:-999.0,
00859 cloudCeilingCullingHeight_>0.0 && (cloudFloorCullingHeight_<=0.0 || cloudCeilingCullingHeight_>cloudFloorCullingHeight_)?cloudCeilingCullingHeight_:999.0);
00860 }
00861
00862 if(assembledCloud->size() && cloudVoxelSize_ > 0 && cloudOutputVoxelized_)
00863 {
00864 assembledCloud = util3d::voxelize(assembledCloud, cloudVoxelSize_);
00865 }
00866
00867 ROS_INFO("Assembled %d clouds (%fs)", count, time.ticks());
00868
00869 if(assembledCloud->size())
00870 {
00871 sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
00872 pcl::toROSMsg(*assembledCloud, *cloudMsg);
00873 cloudMsg->header.stamp = stamp;
00874 cloudMsg->header.frame_id = mapFrameId;
00875 cloudMapPub_.publish(cloudMsg);
00876 }
00877 else if(poses.size() - negativePoses.size())
00878 {
00879 ROS_WARN("Cloud map is empty! (poses=%d clouds=%d)", (int)poses.size(), (int)clouds_.size());
00880 }
00881 }
00882 else if(mapCacheCleanup_)
00883 {
00884 clouds_.clear();
00885 cameraModels_.clear();
00886 }
00887 #ifdef WITH_OCTOMAP_ROS
00888 #ifdef RTABMAP_OCTOMAP
00889 if(octoMapPubBin_.getNumSubscribers() ||
00890 octoMapPubFull_.getNumSubscribers() ||
00891 octoMapCloud_.getNumSubscribers() ||
00892 octoMapEmptySpace_.getNumSubscribers() ||
00893 octoMapProj_.getNumSubscribers())
00894 {
00895 if(octoMapPubBin_.getNumSubscribers())
00896 {
00897 octomap_msgs::Octomap msg;
00898 octomap_msgs::binaryMapToMsg(*octomap_->octree(), msg);
00899 msg.header.frame_id = mapFrameId;
00900 msg.header.stamp = stamp;
00901 octoMapPubBin_.publish(msg);
00902 }
00903 if(octoMapPubFull_.getNumSubscribers())
00904 {
00905 octomap_msgs::Octomap msg;
00906 octomap_msgs::fullMapToMsg(*octomap_->octree(), msg);
00907 msg.header.frame_id = mapFrameId;
00908 msg.header.stamp = stamp;
00909 octoMapPubFull_.publish(msg);
00910 }
00911 if(octoMapCloud_.getNumSubscribers() || octoMapEmptySpace_.getNumSubscribers())
00912 {
00913 sensor_msgs::PointCloud2 msg;
00914 pcl::IndicesPtr obstacles(new std::vector<int>);
00915 pcl::IndicesPtr ground(new std::vector<int>);
00916 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(octomapTreeDepth_, obstacles.get(), ground.get());
00917
00918 if(octoMapCloud_.getNumSubscribers())
00919 {
00920 pcl::PointCloud<pcl::PointXYZRGB> cloudObstacles;
00921 pcl::copyPointCloud(*cloud, *obstacles, cloudObstacles);
00922 pcl::toROSMsg(cloudObstacles, msg);
00923 msg.header.frame_id = mapFrameId;
00924 msg.header.stamp = stamp;
00925 octoMapCloud_.publish(msg);
00926 }
00927 if(octoMapEmptySpace_.getNumSubscribers())
00928 {
00929 pcl::PointCloud<pcl::PointXYZRGB> cloudGround;
00930 pcl::copyPointCloud(*cloud, *ground, cloudGround);
00931 pcl::toROSMsg(cloudGround, msg);
00932 msg.header.frame_id = mapFrameId;
00933 msg.header.stamp = stamp;
00934 octoMapEmptySpace_.publish(msg);
00935 }
00936 }
00937 if(octoMapProj_.getNumSubscribers())
00938 {
00939
00940 float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
00941 cv::Mat pixels = octomap_->createProjectionMap(xMin, yMin, gridCellSize, gridSize_);
00942
00943 if(!pixels.empty())
00944 {
00945
00946 nav_msgs::OccupancyGrid map;
00947 map.info.resolution = gridCellSize;
00948 map.info.origin.position.x = 0.0;
00949 map.info.origin.position.y = 0.0;
00950 map.info.origin.position.z = 0.0;
00951 map.info.origin.orientation.x = 0.0;
00952 map.info.origin.orientation.y = 0.0;
00953 map.info.origin.orientation.z = 0.0;
00954 map.info.origin.orientation.w = 1.0;
00955
00956 map.info.width = pixels.cols;
00957 map.info.height = pixels.rows;
00958 map.info.origin.position.x = xMin;
00959 map.info.origin.position.y = yMin;
00960 map.data.resize(map.info.width * map.info.height);
00961
00962 memcpy(map.data.data(), pixels.data, map.info.width * map.info.height);
00963
00964 map.header.frame_id = mapFrameId;
00965 map.header.stamp = stamp;
00966
00967 octoMapProj_.publish(map);
00968 }
00969 else if(poses.size())
00970 {
00971 ROS_WARN("Projection map is empty! (proj maps=%d)", (int)projMaps_.size());
00972 }
00973 }
00974 }
00975 else
00976 {
00977 octomap_->clear();
00978 }
00979 #endif
00980 #endif
00981
00982
00983 if(scanMapPub_.getNumSubscribers())
00984 {
00985
00986 UTimer time;
00987 pcl::PointCloud<pcl::PointXYZ>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZ>);
00988 int count = 0;
00989 std::list<std::pair<int, Transform> > negativePoses;
00990 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
00991 {
00992 if(iter->first > 0)
00993 {
00994 std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator jter = scans_.find(iter->first);
00995 if(jter != scans_.end() && jter->second->size())
00996 {
00997 pcl::PointCloud<pcl::PointXYZ>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
00998 *assembledCloud+=*transformed;
00999 ++count;
01000 }
01001 }
01002
01003 }
01004
01005 if(assembledCloud->size())
01006 {
01007 if(assembledCloud->size() && scanVoxelSize_ > 0 && scanOutputVoxelized_)
01008 {
01009 assembledCloud = util3d::voxelize(assembledCloud, scanVoxelSize_);
01010 }
01011
01012 ROS_INFO("Assembled %d scans (%fs)", count, time.ticks());
01013
01014 sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
01015 pcl::toROSMsg(*assembledCloud, *cloudMsg);
01016 cloudMsg->header.stamp = stamp;
01017 cloudMsg->header.frame_id = mapFrameId;
01018 scanMapPub_.publish(cloudMsg);
01019 }
01020 else if(poses.size())
01021 {
01022 ROS_WARN("Scan map is empty! (poses=%d, scans=%d)", (int)poses.size(), (int)scans_.size());
01023 }
01024 }
01025 else if(mapCacheCleanup_)
01026 {
01027 scans_.clear();
01028 }
01029
01030 if(projMapPub_.getNumSubscribers())
01031 {
01032
01033 float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
01034 cv::Mat pixels = this->generateProjMap(poses, xMin, yMin, gridCellSize);
01035
01036 if(!pixels.empty())
01037 {
01038
01039 nav_msgs::OccupancyGrid map;
01040 map.info.resolution = gridCellSize;
01041 map.info.origin.position.x = 0.0;
01042 map.info.origin.position.y = 0.0;
01043 map.info.origin.position.z = 0.0;
01044 map.info.origin.orientation.x = 0.0;
01045 map.info.origin.orientation.y = 0.0;
01046 map.info.origin.orientation.z = 0.0;
01047 map.info.origin.orientation.w = 1.0;
01048
01049 map.info.width = pixels.cols;
01050 map.info.height = pixels.rows;
01051 map.info.origin.position.x = xMin;
01052 map.info.origin.position.y = yMin;
01053 map.data.resize(map.info.width * map.info.height);
01054
01055 memcpy(map.data.data(), pixels.data, map.info.width * map.info.height);
01056
01057 map.header.frame_id = mapFrameId;
01058 map.header.stamp = stamp;
01059
01060 projMapPub_.publish(map);
01061 }
01062 else if(poses.size())
01063 {
01064 ROS_WARN("Projection map is empty! (proj maps=%d)", (int)projMaps_.size());
01065 }
01066 }
01067 else if(mapCacheCleanup_)
01068 {
01069 projMaps_.clear();
01070 }
01071
01072 if(gridMapPub_.getNumSubscribers())
01073 {
01074
01075 float xMin=0.0f, yMin=0.0f, gridCellSize = 0.05f;
01076 cv::Mat pixels = this->generateGridMap(poses, xMin, yMin, gridCellSize);
01077
01078 if(!pixels.empty())
01079 {
01080
01081 nav_msgs::OccupancyGrid map;
01082 map.info.resolution = gridCellSize;
01083 map.info.origin.position.x = 0.0;
01084 map.info.origin.position.y = 0.0;
01085 map.info.origin.position.z = 0.0;
01086 map.info.origin.orientation.x = 0.0;
01087 map.info.origin.orientation.y = 0.0;
01088 map.info.origin.orientation.z = 0.0;
01089 map.info.origin.orientation.w = 1.0;
01090
01091 map.info.width = pixels.cols;
01092 map.info.height = pixels.rows;
01093 map.info.origin.position.x = xMin;
01094 map.info.origin.position.y = yMin;
01095 map.data.resize(map.info.width * map.info.height);
01096
01097 memcpy(map.data.data(), pixels.data, map.info.width * map.info.height);
01098
01099 map.header.frame_id = mapFrameId;
01100 map.header.stamp = stamp;
01101
01102 gridMapPub_.publish(map);
01103 }
01104 else if(poses.size())
01105 {
01106 ROS_WARN("Grid map is empty! (local maps=%d)", (int)gridMaps_.size());
01107 }
01108 }
01109 else if(mapCacheCleanup_)
01110 {
01111 gridMaps_.clear();
01112 }
01113 }
01114
01115 cv::Mat MapsManager::generateProjMap(
01116 const std::map<int, rtabmap::Transform> & poses,
01117 float & xMin,
01118 float & yMin,
01119 float & gridCellSize)
01120 {
01121 gridCellSize = gridCellSize_;
01122 return util3d::create2DMapFromOccupancyLocalMaps(
01123 poses,
01124 projMaps_,
01125 gridCellSize_,
01126 xMin, yMin,
01127 gridSize_,
01128 gridEroded_);
01129 }
01130
01131 cv::Mat MapsManager::generateGridMap(
01132 const std::map<int, rtabmap::Transform> & poses,
01133 float & xMin,
01134 float & yMin,
01135 float & gridCellSize)
01136 {
01137 gridCellSize = gridCellSize_;
01138 cv::Mat map = util3d::create2DMapFromOccupancyLocalMaps(
01139 poses,
01140 gridMaps_,
01141 gridCellSize_,
01142 xMin, yMin,
01143 gridSize_,
01144 gridEroded_);
01145 return map;
01146 }
01147