Removed grid_map_assembler, map_assembler is now using MapsManager (now use same published topics and mapping parameters than rtabmap), added all optimization parameters to map_optimizer (g2o and GTSAM can be selected)

2026-10-05 17:27:46 +08:00 · 2015-10-31 15:20:57 -04:00
parent cb0ebecd5f
commit 1dfa4e7f95
7 changed files with 212 additions and 491 deletions
@@ -29,13 +29,15 @@

 using namespace rtabmap;

-MapsManager::MapsManager() :
+MapsManager::MapsManager(bool usePublicNamespace) :
 		cloudDecimation_(4),
 		cloudMaxDepth_(4.0), // meters
 		cloudVoxelSize_(0.05), // meters
 		cloudFloorCullingHeight_(0.0),
 		cloudOutputVoxelized_(false),
 		cloudFrustumCulling_(false),
+		scanVoxelSize_(0.0),
+		scanOutputVoxelized_(false),
 		projMaxGroundAngle_(45.0), // degrees
 		projMinClusterSize_(20),
 		projMaxHeight_(2.0), // meters
@@ -58,6 +60,12 @@ MapsManager::MapsManager() :
 	pnh.param("cloud_floor_culling_height", cloudFloorCullingHeight_, cloudFloorCullingHeight_);
 	pnh.param("cloud_output_voxelized", cloudOutputVoxelized_, cloudOutputVoxelized_);
 	pnh.param("cloud_frustum_culling", cloudFrustumCulling_, cloudFrustumCulling_);
+	pnh.param("cloud_noise_filtering_radius", cloudNoiseFilteringRadius_, cloudNoiseFilteringRadius_);
+	pnh.param("cloud_noise_filtering_min_neighbors", cloudNoiseFilteringMinNeighbors_, cloudNoiseFilteringMinNeighbors_);
+
+	// scan map stuff
+	pnh.param("scan_voxel_size", scanVoxelSize_, scanVoxelSize_);
+	pnh.param("scan_output_voxelized", scanOutputVoxelized_, scanOutputVoxelized_);

 	//projection map stuff
 	pnh.param("proj_max_ground_angle", projMaxGroundAngle_, projMaxGroundAngle_);
@@ -75,10 +83,27 @@ MapsManager::MapsManager() :
 	pnh.param("map_filter_angle", mapFilterAngle_, mapFilterAngle_);
 	pnh.param("map_cleanup", mapCacheCleanup_, mapCacheCleanup_);

+	// If true, the last message published on
+	// the map topics will be saved and sent to new subscribers when they
+	// connect
+	bool latch = true;
+	pnh.param("latch", latch, latch);
+
 	// mapping topics
-	cloudMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud_map", 1);
-	projMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("proj_map", 1);
-	gridMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1);
+	if(usePublicNamespace)
+	{
+		cloudMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud_map", 1, latch);
+		projMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("proj_map", 1, latch);
+		gridMapPub_ = nh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1, latch);
+		scanMapPub_ = nh.advertise<sensor_msgs::PointCloud2>("scan_map", 1, latch);
+	}
+	else
+	{
+		cloudMapPub_ = pnh.advertise<sensor_msgs::PointCloud2>("cloud_map", 1, latch);
+		projMapPub_ = pnh.advertise<nav_msgs::OccupancyGrid>("proj_map", 1, latch);
+		gridMapPub_ = pnh.advertise<nav_msgs::OccupancyGrid>("grid_map", 1, latch);
+		scanMapPub_ = pnh.advertise<sensor_msgs::PointCloud2>("scan_map", 1, latch);
+	}
 }

 MapsManager::~MapsManager() {
@@ -97,7 +122,8 @@ bool MapsManager::hasSubscribers() const
 {
 	return  cloudMapPub_.getNumSubscribers() != 0 ||
 			projMapPub_.getNumSubscribers() != 0 ||
-			gridMapPub_.getNumSubscribers() != 0;
+			gridMapPub_.getNumSubscribers() != 0 ||
+			scanMapPub_.getNumSubscribers() != 0;
 }

 std::map<int, Transform> MapsManager::getFilteredPoses(const std::map<int, Transform> & poses)
@@ -117,28 +143,30 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 		bool updateCloud,
 		bool updateProj,
 		bool updateGrid,
+		bool updateScan,
 		const std::map<int, rtabmap::Signature> & signatures)
 {
-	if(!updateCloud && !updateProj && !updateGrid)
+	if(!updateCloud && !updateProj && !updateGrid && !updateScan)
 	{
 		//  all false, udpate only those where we have subscribers
 		updateCloud = cloudMapPub_.getNumSubscribers() != 0;
 		updateProj = projMapPub_.getNumSubscribers() != 0;
 		updateGrid = gridMapPub_.getNumSubscribers() != 0;
+		updateScan = scanMapPub_.getNumSubscribers() != 0;
 	}

 	UDEBUG("Updating map caches...");

 	if(!memory && signatures.size() == 0)
 	{
-		ROS_FATAL("Memory should not be null!?");
+		ROS_ERROR("Memory and signatures should not be both null!?");
 		return std::map<int, rtabmap::Transform>();
 	}

 	std::map<int, rtabmap::Transform> filteredPoses;

 	// update cache
-	if(updateCloud || updateProj || updateGrid)
+	if(updateCloud || updateProj || updateGrid || updateScan)
 	{
 		// filter nodes
 		if(mapFilterRadius_ > 0.0)
@@ -170,11 +198,13 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 				rtabmap::SensorData data;
 				bool rgbDepthRequired = updateCloud && (iter->first < 0 || !uContains(clouds_, iter->first));
 				bool depthRequired = updateProj && (iter->first < 0 || !uContains(projMaps_, iter->first));
-				bool scanRequired = updateGrid && (iter->first < 0 || !uContains(gridMaps_, iter->first));
+				bool gridRequired = updateGrid && (iter->first < 0 || !uContains(gridMaps_, iter->first));
+				bool scanRequired = updateScan && (iter->first < 0 || !uContains(scans_, iter->first));

 				if(rgbDepthRequired ||
 					depthRequired ||
-					scanRequired)
+					scanRequired ||
+					gridRequired)
 				{
 					std::map<int, rtabmap::Signature>::const_iterator findIter = signatures.find(iter->first);
 					if(findIter != signatures.end())
@@ -198,7 +228,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 						data.uncompressData(
 								(rgbDepthRequired||data.stereoCameraModel().isValid()) ? &image:0,
 								(rgbDepthRequired||depthRequired) ? &depth:0,
-								scanRequired?&scan:0);
+								scanRequired||gridRequired?&scan:0);

 						pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB;
 						pcl::PointCloud<pcl::PointXYZ>::Ptr cloudXYZ;
@@ -211,6 +241,13 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 										cloudDecimation_,
 										cloudMaxDepth_,
 										cloudVoxelSize_);
+								if(cloudRGB->size() && cloudNoiseFilteringRadius_ > 0.0 && cloudNoiseFilteringMinNeighbors_ > 0)
+								{
+									pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloudRGB, cloudNoiseFilteringRadius_, cloudNoiseFilteringMinNeighbors_);
+									pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZRGB>);
+									pcl::copyPointCloud(*cloudRGB, *indices, *tmp);
+									cloudRGB = tmp;
+								}
 							}
 							else
 							{
@@ -226,6 +263,13 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 										cloudDecimation_,
 										cloudMaxDepth_,
 										gridCellSize_); // use gridCellSize since this cloud is only for the projection map
+								if(cloudXYZ->size() && cloudNoiseFilteringRadius_ > 0.0 && cloudNoiseFilteringMinNeighbors_ > 0)
+								{
+									pcl::IndicesPtr indices = rtabmap::util3d::radiusFiltering(cloudXYZ, cloudNoiseFilteringRadius_, cloudNoiseFilteringMinNeighbors_);
+									pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
+									pcl::copyPointCloud(*cloudXYZ, *indices, *tmp);
+									cloudXYZ = tmp;
+								}
 							}
 							else
 							{
@@ -273,7 +317,7 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 								{
 									cloudClipped = util3d::passThrough(cloudClipped, "z", std::numeric_limits<int>::min(), projMaxHeight_);
 								}
-								if(cloudClipped->size())
+								if(cloudClipped->size() && gridCellSize_ > cloudVoxelSize_)
 								{
 									cloudClipped = util3d::voxelize(cloudClipped, gridCellSize_);
 									util3d::occupancy2DFromCloud3D<pcl::PointXYZRGB>(cloudClipped, ground, obstacles, gridCellSize_, projMaxGroundAngle_*M_PI/180.0, projMinClusterSize_);
@@ -294,11 +338,30 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
 							uInsert(projMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
 						}

-						if(scanRequired)
+						if(scanRequired || gridRequired)
 						{
-							cv::Mat ground, obstacles;
-							util3d::occupancy2DFromLaserScan(scan, ground, obstacles, gridCellSize_, data.id() < 0 || gridUnknownSpaceFilled_, data.laserScanMaxRange());
-							uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
+							if(scan.cols && (gridRequired || scanVoxelSize_ > 0.0))
+							{
+								pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud = util3d::laserScanToPointCloud(scan);
+								if(scanVoxelSize_ > 0.0)
+								{
+									scanCloud = util3d::voxelize(scanCloud, scanVoxelSize_);
+									if(gridRequired)
+									{
+										scan = util3d::laserScanFromPointCloud(*scanCloud);
+									}
+								}
+								if(scanRequired)
+								{
+									uInsert(scans_, std::make_pair(iter->first, scanCloud));
+								}
+							}
+							if(gridRequired)
+							{
+								cv::Mat ground, obstacles;
+								util3d::occupancy2DFromLaserScan(scan, ground, obstacles, gridCellSize_, data.id() < 0 || gridUnknownSpaceFilled_, data.laserScanMaxRange());
+								uInsert(gridMaps_, std::make_pair(iter->first, std::make_pair(ground, obstacles)));
+							}
 						}
 					}
 					else
@@ -428,20 +491,23 @@ void MapsManager::publishMaps(
 										cloudMaxDepth_>0.0?cloudMaxDepth_:999999.,
 										true);
 								//ROS_INFO("Frustum culling %d ->%d", size, (int)assembledCloud->size());
-								pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
-								*assembledCloud+=*transformed;
+								if(jter->second->size())
+								{
+									pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
+									*assembledCloud+=*transformed;
+								}
 							}
 						}
 					}
 				}
 			}

-			if(cloudFloorCullingHeight_ > 0.0)
+			if(assembledCloud->size() && cloudFloorCullingHeight_ > 0.0)
 			{
 				assembledCloud = util3d::passThrough(assembledCloud, "z", cloudFloorCullingHeight_, 99999.0f);
 			}

-			if(cloudVoxelSize_ > 0 && cloudOutputVoxelized_)
+			if(assembledCloud->size() && cloudVoxelSize_ > 0 && cloudOutputVoxelized_)
 			{
 				assembledCloud = util3d::voxelize(assembledCloud, cloudVoxelSize_);
 			}
@@ -456,7 +522,7 @@ void MapsManager::publishMaps(
 		}
 		else if(poses.size())
 		{
-			ROS_WARN("Cloud map is empty! (clouds=%d)", (int)clouds_.size());
+			ROS_WARN("Cloud map is empty! (poses=%d clouds=%d)", (int)poses.size(), (int)clouds_.size());
 		}
 	}
 	else if(mapCacheCleanup_)
@@ -465,6 +531,53 @@ void MapsManager::publishMaps(
 		cameraModels_.clear();
 	}

+	if(scanMapPub_.getNumSubscribers())
+	{
+		// generate the assembled scan cloud!
+		UTimer time;
+		pcl::PointCloud<pcl::PointXYZ>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZ>);
+		int count = 0;
+		std::list<std::pair<int, Transform> > negativePoses;
+		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			if(iter->first > 0)
+			{
+				std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator jter = scans_.find(iter->first);
+				if(jter != scans_.end() && jter->second->size())
+				{
+					pcl::PointCloud<pcl::PointXYZ>::Ptr transformed = util3d::transformPointCloud(jter->second, iter->second);
+					*assembledCloud+=*transformed;
+					++count;
+				}
+			}
+			// negative poses are not used
+		}
+
+		if(assembledCloud->size())
+		{
+			if(assembledCloud->size() && scanVoxelSize_ > 0 && scanOutputVoxelized_)
+			{
+				assembledCloud = util3d::voxelize(assembledCloud, scanVoxelSize_);
+			}
+
+			ROS_INFO("Assembled %d scans (%fs)", count, time.ticks());
+
+			sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
+			pcl::toROSMsg(*assembledCloud, *cloudMsg);
+			cloudMsg->header.stamp = stamp;
+			cloudMsg->header.frame_id = mapFrameId;
+			scanMapPub_.publish(cloudMsg);
+		}
+		else if(poses.size())
+		{
+			ROS_WARN("Scan map is empty! (poses=%d, scans=%d)", (int)poses.size(), (int)scans_.size());
+		}
+	}
+	else if(mapCacheCleanup_)
+	{
+		scans_.clear();
+	}
+
 	if(projMapPub_.getNumSubscribers())
 	{
 		// create the projection map