New optional param 'metric_map_use_only_these_layers' to use only a s…

…ubset of the .mm map layers

mrpt_pf_localization/include/mrpt_pf_localization/mrpt_pf_localization_core.h

@@ -61,8 +61,12 @@ class PFLocalizationCore : public mrpt::system::COutputLogger
 		 */
 		std::optional<mrpt::poses::CPose3DPDFGaussian> initial_pose;
 
+		std::set<std::string> metric_map_use_only_these_layers;
+
 		mrpt::maps::CMultiMetricMap::Ptr metric_map;  //!< Empty=uninitialized
 		std::optional<mp2p_icp::metric_map_t::Georeferencing> georeferencing;
+
+		// used internally for relocalization only:
 		std::vector<std::string> metric_map_layer_names;
 
 		/** Shows a custom MRPT GUI with the PF and map state

mrpt_pf_localization/params/default.config.yaml

@@ -73,6 +73,10 @@
         alfa4_trans_rot: 0.1
         additional_std_XY: 0.01 # [m]
         additional_std_phi: 0.2 # [deg]
+
+    # Optional: Can be used to use only a subset of the input metric-map (.mm)
+    # map layers for localization. Layer names must be separated by commas.
+    #metric_map_use_only_these_layers: 'map'
 
     # For SE(2) mode: Uncertainty motion model to use when NO odometry has
     # been received.

mrpt_pf_localization/src/mrpt_pf_localization/mrpt_pf_localization_core.cpp

@@ -125,6 +125,16 @@ void PFLocalizationCore::Parameters::load_from(
 	MCP_LOAD_REQ(params, use_se3_pf);
 	MCP_LOAD_OPT(params, gui_camera_follow_robot);
 
+	if (params.has("metric_map_use_only_these_layers"))
+	{
+		const auto s =
+			params["metric_map_use_only_these_layers"].as<std::string>();
+		std::vector<std::string> lstLayers;
+		mrpt::system::tokenize(s, ", ", lstLayers);
+		for (const auto& l : lstLayers)
+			metric_map_use_only_these_layers.insert(l);
+	}
+
 	// motion_model_2d
 	ASSERT_(params.has("motion_model_2d"));
 	load_motion_model2d_from(params["motion_model_2d"], motion_model_2d);
@@ -1027,6 +1037,12 @@ void PFLocalizationCore::set_map_from_metric_map(
 	std::vector<std::string> layerNames;
 	for (const auto& [layerName, layerMap] : mm.layers)
 	{
+		// filter by layer?
+		if (!params_.metric_map_use_only_these_layers.empty() &&
+			params_.metric_map_use_only_these_layers.count(layerName) == 0)
+			continue;  // filter out this one
+
+		// use this map layer:
 		mMap->maps.push_back(layerMap);
 		layerNames.push_back(layerName);
 	}