Increased required rtabmap version to 0.20.14. Added detect_more_loop_closures, global_bundle_adjustment and cleanup_local_grids services. rviz MapCloud: added "Download namespace" parameter.

2026-10-07 02:07:45 +08:00 · 2021-09-11 11:39:11 -04:00
parent b2ac4b6ea7
commit a324562ee0
8 changed files with 359 additions and 81 deletions
@@ -60,6 +60,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/DBDriver.h>
 #include <rtabmap/core/Registration.h>
 #include <rtabmap/core/Graph.h>
+#include <rtabmap/core/Optimizer.h>

 #ifdef WITH_OCTOMAP_MSGS
 #ifdef RTABMAP_OCTOMAP
@@ -610,7 +611,7 @@ void CoreWrapper::onInit()
 		Parameters::parse(parameters_, Parameters::kRegForce3DoF(), twoDMapping_);
 	}

-	pnh.param("is_rtabmap_paused", paused_);
+	paused_ = pnh.param("is_rtabmap_paused", paused_);
 	if(paused_)
 	{
 		NODELET_WARN("Node paused... don't forget to call service \"resume\" to start rtabmap.");
@@ -640,7 +641,7 @@ void CoreWrapper::onInit()

 	if(rtabmap_.getMemory())
 	{
-		if(useSavedMap_ && !rtabmap_.getMemory()->isIncremental())
+		if(useSavedMap_)
 		{
 			float xMin, yMin, gridCellSize;
 			cv::Mat map = rtabmap_.getMemory()->load2DMap(xMin, yMin, gridCellSize);
@@ -681,6 +682,9 @@ void CoreWrapper::onInit()
 	loadDatabaseSrv_ = nh.advertiseService("load_database", &CoreWrapper::loadDatabaseCallback, this);
 	triggerNewMapSrv_ = nh.advertiseService("trigger_new_map", &CoreWrapper::triggerNewMapCallback, this);
 	backupDatabase_ = nh.advertiseService("backup", &CoreWrapper::backupDatabaseCallback, this);
+	detectMoreLoopClosuresSrv_ = nh.advertiseService("detect_more_loop_closures", &CoreWrapper::detectMoreLoopClosuresCallback, this);
+	globalBundleAdjustmentSrv_ = nh.advertiseService("global_bundle_adjustment", &CoreWrapper::globalBundleAdjustmentCallback, this);
+	cleanupLocalGridsSrv_ = nh.advertiseService("cleanup_local_grids", &CoreWrapper::cleanupLocalGridsCallback, this);
 	setModeLocalizationSrv_ = nh.advertiseService("set_mode_localization", &CoreWrapper::setModeLocalizationCallback, this);
 	setModeMappingSrv_ = nh.advertiseService("set_mode_mapping", &CoreWrapper::setModeMappingCallback, this);
 	getNodeDataSrv_ = nh.advertiseService("get_node_data", &CoreWrapper::getNodeDataCallback, this);
@@ -3017,6 +3021,207 @@ bool CoreWrapper::backupDatabaseCallback(std_srvs::Empty::Request&, std_srvs::Em
 	return true;
 }

+void CoreWrapper::republishMaps()
+{
+	ros::Time stamp = ros::Time::now();
+	mapsManager_.publishMaps(rtabmap_.getLocalOptimizedPoses(), stamp, mapFrameId_);
+
+	if(mapDataPub_.getNumSubscribers())
+	{
+		rtabmap_ros::MapDataPtr msg(new rtabmap_ros::MapData);
+		msg->header.stamp = stamp;
+		msg->header.frame_id = mapFrameId_;
+
+		rtabmap_ros::mapDataToROS(
+			rtabmap_.getLocalOptimizedPoses(),
+			rtabmap_.getLocalConstraints(),
+			std::map<int, Signature>(),
+			rtabmap_.getMapCorrection(),
+			*msg);
+
+		mapDataPub_.publish(msg);
+	}
+
+	if(mapGraphPub_.getNumSubscribers())
+	{
+		rtabmap_ros::MapGraphPtr msg(new rtabmap_ros::MapGraph);
+		msg->header.stamp = stamp;
+		msg->header.frame_id = mapFrameId_;
+
+		rtabmap_ros::mapGraphToROS(
+		rtabmap_.getLocalOptimizedPoses(),
+		rtabmap_.getLocalConstraints(),
+		rtabmap_.getMapCorrection(),
+		*msg);
+
+		mapGraphPub_.publish(msg);
+	}
+}
+
+bool CoreWrapper::detectMoreLoopClosuresCallback(rtabmap_ros::DetectMoreLoopClosures::Request& req, rtabmap_ros::DetectMoreLoopClosures::Response& res)
+{
+	NODELET_WARN("Detect more loop closures service called");
+
+	UTimer timer;
+	float clusterRadiusMax = 1;
+	float clusterRadiusMin = 0;
+	float clusterAngle = 0;
+	int iterations = 1;
+	bool intraSession = true;
+	bool interSession = true;
+	if(req.cluster_radius_max > 0.0f)
+	{
+		clusterRadiusMax = req.cluster_radius_max;
+	}
+	if(req.cluster_radius_min >= 0.0f)
+	{
+		clusterRadiusMin = req.cluster_radius_min;
+	}
+	if(req.cluster_angle >= 0.0f)
+	{
+		clusterAngle = req.cluster_angle;
+	}
+	if(req.iterations >= 1.0f)
+	{
+		iterations = (int)req.iterations;
+	}
+	if(req.intra_only)
+	{
+		interSession = false;
+	}
+	else if(req.inter_only)
+	{
+		intraSession = false;
+	}
+	NODELET_WARN("Post-Processing service called: Detecting more loop closures "
+			"(max radius=%f, min radius=%f, angle=%f, iterations=%d, intra=%s, inter=%s)...",
+			clusterRadiusMax,
+			clusterRadiusMin,
+			clusterAngle,
+			iterations,
+			intraSession?"true":"false",
+			interSession?"true":"false");
+	res.detected = rtabmap_.detectMoreLoopClosures(
+			clusterRadiusMax,
+			clusterAngle*M_PI/180.0,
+			iterations,
+			intraSession,
+			interSession,
+			0,
+			clusterRadiusMin);
+	if(res.detected<0)
+	{
+		NODELET_ERROR("Post-Processing: Detecting more loop closures failed!");
+	}
+	else
+	{
+		NODELET_WARN("Post-Processing: Detected %d loop closures! (%fs)", res.detected, timer.ticks());
+
+		if(res.detected>0)
+		{
+			republishMaps();
+		}
+		return true;
+	}
+	return false;
+}
+
+bool CoreWrapper::cleanupLocalGridsCallback(rtabmap_ros::CleanupLocalGrids::Request& req, rtabmap_ros::CleanupLocalGrids::Response& res)
+{
+	NODELET_WARN("Cleanup local grids service called");
+	UTimer timer;
+	int radius = 1;
+	bool filterScans = false;
+	if(req.radius > 1.0f)
+	{
+		radius = (int)req.radius;
+	}
+	filterScans = req.filter_scans;
+	float xMin, yMin, gridCellSize;
+	cv::Mat map = mapsManager_.getGridMap(xMin, yMin, gridCellSize);
+	if(map.empty())
+	{
+		NODELET_ERROR("Post-Processing: Cleanup local grids failed! There is no optimized map.");
+		return false;
+	}
+	std::map<int, Transform> poses = rtabmap_.getLocalOptimizedPoses();
+	NODELET_WARN("Post-Processing: Cleanup local grids... (radius=%d, filter scans=%s)",
+			radius,
+			filterScans?"true":"false");
+	res.modified = rtabmap_.cleanupLocalGrids(poses, map, xMin, yMin, gridCellSize, radius, filterScans);
+	if(res.modified<0)
+	{
+		NODELET_ERROR("Post-Processing: Cleanup local grids failed!");
+	}
+	else
+	{
+		if(filterScans)
+		{
+			NODELET_WARN("Post-Processing: %d grids and scans modified! (%fs)", res.modified, timer.ticks());
+		}
+		else
+		{
+			NODELET_WARN("Post-Processing: %d grids modified! (%fs)", res.modified, timer.ticks());
+		}
+		if(res.modified > 0)
+		{
+			// We should update MapsManager's cache with the modifications
+			mapsManager_.clear();
+			mapsManager_.set2DMap(map, xMin, yMin, gridCellSize, rtabmap_.getLocalOptimizedPoses(), rtabmap_.getMemory());
+
+			republishMaps();
+		}
+		return true;
+	}
+
+	return false;
+}
+bool CoreWrapper::globalBundleAdjustmentCallback(rtabmap_ros::GlobalBundleAdjustment::Request& req, rtabmap_ros::GlobalBundleAdjustment::Response& res)
+{
+	NODELET_WARN("Global bundle adjustment service called");
+
+	UTimer timer;
+	int optimizer = (int)Optimizer::kTypeG2O; // g2o
+	int iterations = Parameters::defaultOptimizerIterations();
+	float pixelVariance = Parameters::defaultg2oPixelVariance();
+	bool rematchFeatures = true;
+	Parameters::parse(parameters_, Parameters::kOptimizerIterations(), iterations);
+	Parameters::parse(parameters_, Parameters::kg2oPixelVariance(), pixelVariance);
+	if(req.type == 1.0f)
+	{
+		optimizer = (int)Optimizer::kTypeCVSBA;
+	}
+	if(req.iterations >= 1.0f)
+	{
+		iterations = req.iterations;
+	}
+	if(req.pixel_variance > 0.0f)
+	{
+		pixelVariance = req.pixel_variance;
+	}
+	rematchFeatures = !req.voc_matches;
+
+	NODELET_WARN("Post-Processing: Global Bundle Adjustment... "
+			"(Optimizer=%s, iterations=%d, pixel variance=%f, rematch=%s)...",
+			optimizer==Optimizer::kTypeG2O?"g2o":"cvsba",
+			iterations,
+			pixelVariance,
+			rematchFeatures?"true":"false");
+	bool success = rtabmap_.globalBundleAdjustment((Optimizer::Type)optimizer, iterations, pixelVariance, rematchFeatures);
+	if(!success)
+	{
+		NODELET_ERROR("Post-Processing: Global Bundle Adjustment failed!");
+	}
+	else
+	{
+		NODELET_WARN("Post-Processing: Global Bundle Adjustment... done! (%fs)", timer.ticks());
+		republishMaps();
+		return true;
+	}
+
+	return false;
+}
+
 bool CoreWrapper::setModeLocalizationCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
 {
 	NODELET_INFO("rtabmap: Set localization mode");
@@ -186,6 +186,8 @@ MapCloudDisplay::MapCloudDisplay()
 	node_filtering_angle_->setMin( 0.0f );
 	node_filtering_angle_->setMax( 359.0f );

+	download_namespace = new rviz::StringProperty("Download namespace", "rtabmap", "Namespace used to call Download services below", this);
+
 	download_map_ = new rviz::BoolProperty( "Download map", false,
 										 "Download the optimized global map using rtabmap/GetMap service. This will force to re-create all clouds.",
 										 this, SLOT( downloadMap() ), this );
@@ -527,50 +529,65 @@ void MapCloudDisplay::updateCloudParameters()
 	// do nothing... only take effect on next generated clouds
 }

+void MapCloudDisplay::downloadMap(bool graphOnly)
+{
+	rtabmap_ros::GetMap getMapSrv;
+	getMapSrv.request.global = false;
+	getMapSrv.request.optimized = true;
+	getMapSrv.request.graphOnly = graphOnly;
+	std::string rtabmapNs = download_namespace->getStdString();
+	ros::NodeHandle nh;
+	std::string srvName = nh.resolveName(uFormat("%s/get_map_data", rtabmapNs.c_str()));
+	QMessageBox * messageBox = new QMessageBox(
+			QMessageBox::NoIcon,
+			tr("Calling \"%1\" service...").arg(srvName.c_str()),
+			tr("Downloading the map... please wait (rviz could become gray!)"),
+			QMessageBox::NoButton);
+	messageBox->setAttribute(Qt::WA_DeleteOnClose, true);
+	messageBox->show();
+	QApplication::processEvents();
+	uSleep(100); // hack make sure the text in the QMessageBox is shown...
+	QApplication::processEvents();
+	if(!ros::service::call(srvName, getMapSrv))
+	{
+		ROS_ERROR("MapCloudDisplay: Can't call \"%s\" service. "
+				  "Tip: if rtabmap node is not in \"%s\" namespace, you can "
+				  "change the \"Download namespace\" option.",
+				  srvName.c_str(),
+				  rtabmapNs.c_str());
+		messageBox->setText(tr("MapCloudDisplay: Can't call \"%1\" service. "
+				  "Tip: if rtabmap node is not in \"%2\" namespace, you can "
+				  "change the \"Download namespace\" option.").
+				  arg(srvName.c_str()).arg(rtabmapNs.c_str()));
+	}
+	else if(graphOnly)
+	{
+		messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(getMapSrv.response.data.graph.poses.size()));
+		QApplication::processEvents();
+		processMapData(getMapSrv.response.data);
+		messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(getMapSrv.response.data.graph.poses.size()));
+
+		QTimer::singleShot(1000, messageBox, SLOT(close()));
+	}
+	else
+	{
+		messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...")
+				.arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size()));
+		QApplication::processEvents();
+		this->reset();
+		processMapData(getMapSrv.response.data);
+		messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!")
+				.arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size()));
+
+		QTimer::singleShot(1000, messageBox, SLOT(close()));
+	}
+}
+
 void MapCloudDisplay::downloadMap()
 {
 	if(download_map_->getBool())
 	{
-		rtabmap_ros::GetMap getMapSrv;
-		getMapSrv.request.global = true;
-		getMapSrv.request.optimized = true;
-		getMapSrv.request.graphOnly = false;
-		ros::NodeHandle nh;
-		QMessageBox * messageBox = new QMessageBox(
-				QMessageBox::NoIcon,
-				tr("Calling \"%1\" service...").arg(nh.resolveName("rtabmap/get_map_data").c_str()),
-				tr("Downloading the map... please wait (rviz could become gray!)"),
-				QMessageBox::NoButton);
-		messageBox->setAttribute(Qt::WA_DeleteOnClose, true);
-		messageBox->show();
-		QApplication::processEvents();
-		uSleep(100); // hack make sure the text in the QMessageBox is shown...
-		QApplication::processEvents();
-		if(!ros::service::call("rtabmap/get_map_data", getMapSrv))
-		{
-			ROS_ERROR("MapCloudDisplay: Can't call \"%s\" service. "
-					  "Tip: if rtabmap node is not in rtabmap namespace, you can remap the service "
-					  "to \"get_map_data\" in the launch "
-					  "file like: <remap from=\"rtabmap/get_map_data\" to=\"get_map_data\"/>.",
-					  nh.resolveName("rtabmap/get_map_data").c_str());
-			messageBox->setText(tr("MapCloudDisplay: Can't call \"%1\" service. "
-					  "Tip: if rtabmap node is not in rtabmap namespace, you can remap the service "
-					  "to \"get_map_data\" in the launch "
-					  "file like: <remap from=\"rtabmap/get_map_data\" to=\"get_map_data\"/>.").
-					  arg(nh.resolveName("rtabmap/get_map_data").c_str()));
-		}
-		else
-		{
-			messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)...")
-					.arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size()));
-			QApplication::processEvents();
-			this->reset();
-			processMapData(getMapSrv.response.data);
-			messageBox->setText(tr("Creating all clouds (%1 poses and %2 clouds downloaded)... done!")
-					.arg(getMapSrv.response.data.graph.poses.size()).arg(getMapSrv.response.data.nodes.size()));
-
-			QTimer::singleShot(1000, messageBox, SLOT(close()));
-		}
+		downloadMap(false);
 		download_map_->blockSignals(true);
 		download_map_->setBool(false);
 		download_map_->blockSignals(false);
@@ -589,43 +606,7 @@ void MapCloudDisplay::downloadGraph()
 {
 	if(download_graph_->getBool())
 	{
-		rtabmap_ros::GetMap getMapSrv;
-		getMapSrv.request.global = true;
-		getMapSrv.request.optimized = true;
-		getMapSrv.request.graphOnly = true;
-		ros::NodeHandle nh;
-		QMessageBox * messageBox = new QMessageBox(
-				QMessageBox::NoIcon,
-				tr("Calling \"%1\" service...").arg(nh.resolveName("rtabmap/get_map_data").c_str()),
-				tr("Downloading the graph... please wait (rviz could become gray!)"),
-				QMessageBox::NoButton);
-		messageBox->setAttribute(Qt::WA_DeleteOnClose, true);
-		messageBox->show();
-		QApplication::processEvents();
-		uSleep(100); // hack make sure the text in the QMessageBox is shown...
-		QApplication::processEvents();
-		if(!ros::service::call("rtabmap/get_map_data", getMapSrv))
-		{
-			ROS_ERROR("MapCloudDisplay: Can't call \"%s\" service. "
-					  "Tip: if rtabmap node is not in rtabmap namespace, you can remap the service "
-					  "to \"get_map_data\" in the launch "
-					  "file like: <remap from=\"rtabmap/get_map_data\" to=\"get_map_data\"/>.",
-					  nh.resolveName("rtabmap/get_map_data").c_str());
-			messageBox->setText(tr("MapCloudDisplay: Can't call \"%1\" service. "
-					  "Tip: if rtabmap node is not in rtabmap namespace, you can remap the service "
-					  "to \"get_map_data\" in the launch "
-					  "file like: <remap from=\"rtabmap/get_map_data\" to=\"get_map_data\"/>.").
-					  arg(nh.resolveName("rtabmap/get_map_data").c_str()));
-		}
-		else
-		{
-			messageBox->setText(tr("Updating the map (%1 nodes downloaded)...").arg(getMapSrv.response.data.graph.poses.size()));
-			QApplication::processEvents();
-			processMapData(getMapSrv.response.data);
-			messageBox->setText(tr("Updating the map (%1 nodes downloaded)... done!").arg(getMapSrv.response.data.graph.poses.size()));
-
-			QTimer::singleShot(1000, messageBox, SLOT(close()));
-		}
+		downloadMap(true);
 		download_graph_->blockSignals(true);
 		download_graph_->setBool(false);
 		download_graph_->blockSignals(false);
@@ -759,7 +740,10 @@ void MapCloudDisplay::update( float wall_dt, float ros_dt )
 					}
 					else
 					{
-						ROS_ERROR("MapCloudDisplay: Could not update pose of node %d", it->first);
+						ROS_ERROR("MapCloudDisplay: Could not update pose of node %d (cannot transform pose in target frame id \"%s\", set fixed frame in global options to \"%s\")",
+								it->first,
+								cloudInfoIt->second->message_->header.frame_id.c_str(),
+								cloudInfoIt->second->message_->header.frame_id.c_str());
 					}

 				}
@@ -119,6 +119,7 @@ public:
 	rviz::FloatProperty* cloud_filter_ceiling_height_;
 	rviz::FloatProperty* node_filtering_radius_;
 	rviz::FloatProperty* node_filtering_angle_;
+	rviz::StringProperty * download_namespace;
 	rviz::BoolProperty* download_map_;
 	rviz::BoolProperty* download_graph_;

@@ -145,6 +146,7 @@ protected:
 	virtual void processMessage( const rtabmap_ros::MapDataConstPtr& cloud );

 private:
+	void downloadMap(bool graphOnly);
 	void processMapData(const rtabmap_ros::MapData& map);

 	/**