diff --git a/mrpt_pf_localization/CMakeLists.txt b/mrpt_pf_localization/CMakeLists.txt index 033dbae..e0d2d35 100644 --- a/mrpt_pf_localization/CMakeLists.txt +++ b/mrpt_pf_localization/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(mrpt_msgs_bridge REQUIRED) find_package(sensor_msgs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(mp2p_icp_map REQUIRED) +find_package(mp2p_icp_filters REQUIRED) find_package(mola_relocalization) # OPTIONAL find_package(mrpt-ros2bridge REQUIRED) @@ -56,6 +57,7 @@ target_link_libraries(${PROJECT_NAME}_core mrpt::slam mrpt::ros2bridge mola::mp2p_icp_map + mola::mp2p_icp_filters ) if (mola_relocalization_FOUND) target_link_libraries(${PROJECT_NAME}_core diff --git a/mrpt_pf_localization/include/mrpt_pf_localization/mrpt_pf_localization_core.h b/mrpt_pf_localization/include/mrpt_pf_localization/mrpt_pf_localization_core.h index e0d7e38..fc69b68 100644 --- a/mrpt_pf_localization/include/mrpt_pf_localization/mrpt_pf_localization_core.h +++ b/mrpt_pf_localization/include/mrpt_pf_localization/mrpt_pf_localization_core.h @@ -8,7 +8,10 @@ #pragma once +#include +#include #include +#include #include #include #include @@ -60,6 +63,7 @@ class PFLocalizationCore : public mrpt::system::COutputLogger mrpt::maps::CMultiMetricMap::Ptr metric_map; //!< Empty=uninitialized std::optional georeferencing; + std::vector metric_map_layer_names; /** Shows a custom MRPT GUI with the PF and map state * Can be changed at any moment. @@ -143,6 +147,11 @@ class PFLocalizationCore : public mrpt::system::COutputLogger double relocalization_resolution_xy = 0.50; // [m] double relocalization_resolution_phi = 0.20; // [rad] + mp2p_icp::ICP::Ptr relocalization_icp; + mp2p_icp::Parameters relocalization_icp_params; + + mp2p_icp_filters::FilterPipeline relocalization_obs_filter; + mp2p_icp::ParameterSource paramSource; /// This method loads all parameters from the YAML, except the /// metric_map (handled in parent class): @@ -202,7 +211,8 @@ class PFLocalizationCore : public mrpt::system::COutputLogger void set_map_from_metric_map( const mrpt::maps::CMultiMetricMap::Ptr& metricMap, const std::optional& - georeferencing = std::nullopt); + georeferencing = std::nullopt, + const std::vector& layerNames = {}); /** Defines the map to use from a metric_map_t map, with optional * georeferencing. diff --git a/mrpt_pf_localization/params/default.config.yaml b/mrpt_pf_localization/params/default.config.yaml index 23203a3..7644776 100644 --- a/mrpt_pf_localization/params/default.config.yaml +++ b/mrpt_pf_localization/params/default.config.yaml @@ -49,14 +49,6 @@ # samples around the GNNS prediction: gnns_samples_num_sigmas: 6.0 - # After relocalization runs, what top percentile of the best matched - # poses are to be taken as real potential robot poses for tracking. - relocalization_best_percentile: 0.95 - - relocalization_resolution_xy: 0.25 # [m] - relocalization_resolution_phi: 10.0 # [deg] - - # For SE(2) mode: Uncertainty motion model for regular odometry-based motion # See docs for mrpt::obs::CActionRobotMovement2D::TMotionModelOptions or https://docs.mrpt.org/reference/latest/tutorial-motion-models.html motion_model_2d: @@ -154,3 +146,74 @@ LF_maxRange: 81.0 LF_maxCorrsDistance: 1.0 LF_decimation: 1 + + + # After relocalization runs, what top percentile of the best matched + # poses are to be taken as real potential robot poses for tracking. + relocalization_best_percentile: 0.95 + + relocalization_resolution_xy: 0.25 # [m] + relocalization_resolution_phi: 10.0 # [deg] + + relocalization_icp_pipeline: + # mp2p_icp ICP pipeline configuration. + # YAML configuration file for use with the CLI tool mp2p-icp-run or + # programmatically from function mp2p_icp::icp_pipeline_from_yaml() + class_name: mp2p_icp::ICP + + # See: mp2p_icp::Parameter + params: + maxIterations: 300 + minAbsStep_trans: 1e-4 + minAbsStep_rot: 5e-5 + + #debugPrintIterationProgress: true # Print iteration progress + #generateDebugFiles: true # Can be override with env var "MP2P_ICP_GENERATE_DEBUG_FILES=1" + saveIterationDetails: false # Store partial solutions and pairings for each ICP iteration + decimationIterationDetails: 3 + debugFileNameFormat: "/tmp/pf-icp-run-$UNIQUE_ID-local_$LOCAL_ID$LOCAL_LABEL-to-global_$GLOBAL_ID$GLOBAL_LABEL.icplog" + decimationDebugFiles: 1 + + solvers: + - class: mp2p_icp::Solver_GaussNewton + params: + maxIterations: 2 + robustKernel: 'RobustKernel::GemanMcClure' + #robustKernelParam: '0.5*ADAPTIVE_THRESHOLD_SIGMA' # [m] # (adaptive) + robustKernelParam: '0.5*max(0.5*ADAPTIVE_THRESHOLD_SIGMA, 2.0*ADAPTIVE_THRESHOLD_SIGMA-(2.0*ADAPTIVE_THRESHOLD_SIGMA-0.5*ADAPTIVE_THRESHOLD_SIGMA)*ICP_ITERATION/30)' + #innerLoopVerbose: true + + # Sequence of one or more pairs (class, params) defining mp2p_icp::Matcher + # instances to pair geometric entities between pointclouds. + matchers: + - class: mp2p_icp::Matcher_Points_DistanceThreshold + params: + #threshold: '2.0*ADAPTIVE_THRESHOLD_SIGMA' # [m] + threshold: '2.0*max(0.5*ADAPTIVE_THRESHOLD_SIGMA, 2.0*ADAPTIVE_THRESHOLD_SIGMA-(2.0*ADAPTIVE_THRESHOLD_SIGMA-0.5*ADAPTIVE_THRESHOLD_SIGMA)*ICP_ITERATION/30)' + thresholdAngularDeg: 0 # deg + pairingsPerPoint: 1 + allowMatchAlreadyMatchedGlobalPoints: true # faster + pointLayerMatches: + - {global: "localmap", local: "decimated_for_icp", weight: 1.0} + + quality: + - class: mp2p_icp::QualityEvaluator_PairedRatio + params: + ~ # none required + + # This pipeline will be applied to the (probably already-filtered) input pointcloud that + # arrived at the pf_localization node, which will be exposed here as a layer "raw": + relocalization_observation_pipeline: + # Filters: + # + # One filter object will be created for each entry, instancing the given class, + # and with the given parameters. Filters are run in definition order on the + # input metric_map_t object. + # + - class_name: mp2p_icp_filters::FilterDecimateVoxels + params: + input_pointcloud_layer: 'raw' + output_pointcloud_layer: 'decimated_for_icp' + voxel_filter_resolution: 1.5 # [m] + decimate_method: DecimateMethod::FirstPoint + #decimate_method: DecimateMethod::ClosestToAverage diff --git a/mrpt_pf_localization/src/mrpt_pf_localization/mrpt_pf_localization_core.cpp b/mrpt_pf_localization/src/mrpt_pf_localization/mrpt_pf_localization_core.cpp index 5d9a332..26aba5e 100644 --- a/mrpt_pf_localization/src/mrpt_pf_localization/mrpt_pf_localization_core.cpp +++ b/mrpt_pf_localization/src/mrpt_pf_localization/mrpt_pf_localization_core.cpp @@ -6,6 +6,7 @@ | All rights reserved. Released under BSD 3-Clause license. See LICENSE | +------------------------------------------------------------------------+ */ +#include #include #include #include @@ -13,6 +14,7 @@ #include #include #include +#include #include #include #include @@ -249,15 +251,45 @@ void PFLocalizationCore::Parameters::load_from( MCP_LOAD_OPT(params, initialize_from_gnns); MCP_LOAD_OPT(params, samples_drawn_from_gnns); MCP_LOAD_OPT(params, gnns_samples_num_sigmas); + + // relocalization: MCP_LOAD_OPT(params, relocalization_best_percentile); MCP_LOAD_OPT(params, relocalization_resolution_xy); MCP_LOAD_OPT_DEG(params, relocalization_resolution_phi); + +#ifdef HAVE_MOLA_RELOCALIZATION + if (params.has("relocalization_icp_pipeline")) + { + // ICP pipeline: + const auto [icp, icpParams] = mp2p_icp::icp_pipeline_from_yaml( + params["relocalization_icp_pipeline"]); + relocalization_icp = icp; + relocalization_icp_params = icpParams; + + paramSource.updateVariable("ADAPTIVE_THRESHOLD_SIGMA", 15.0); + relocalization_icp->attachToParameterSource(paramSource); + + // filter? + if (params.has("relocalization_observation_pipeline")) + { + relocalization_obs_filter = + mp2p_icp_filters::filter_pipeline_from_yaml( + params["relocalization_observation_pipeline"]); + } + } + else + { + std::cout << "[PFLocalizationCore] Section " + "'relocalization_icp_pipeline' not found in setup yaml " + "file, so this feature will be disabled.\n"; + } +#endif } struct PFLocalizationCore::InternalState::Relocalization { #ifdef HAVE_MOLA_RELOCALIZATION - std::optional pending_se2; + std::optional pending_se2; #endif }; @@ -543,17 +575,43 @@ void PFLocalizationCore::onStateToBeInitialized() #if defined(HAVE_MOLA_RELOCALIZATION) // 2) Use mola_relocalization auto& in = state_.pendingRelocalization->pending_se2.emplace(); - in.corner_min = mrpt::math::TPose2D(pMin); - in.corner_max = mrpt::math::TPose2D(pMax); - // in.observations: to be populated in running state - in.resolution_xy = params_.relocalization_resolution_xy; - in.resolution_phi = params_.relocalization_resolution_phi; + in.icp_minimum_quality = 0.70; + + in.icp_parameters = params_.relocalization_icp_params; + in.icp_pipeline = {params_.relocalization_icp}; + + MRPT_LOG_INFO_STREAM( + "Setting up relocalization with: pMin=" << pMin + << " pMax=" << pMax); + const auto pDiag = pMax - pMin; + + in.initial_guess_lattice.corner_min = mrpt::math::TPose2D(pMin); + in.initial_guess_lattice.corner_max = mrpt::math::TPose2D(pMax); + in.initial_guess_lattice.resolution_xy = + std::max(0.5, pDiag.norm() / 3); + in.initial_guess_lattice.resolution_phi = + std::max(mrpt::DEG2RAD(5.0), (pMax.yaw - pMin.yaw) / 3); + + in.output_lattice.resolution_xyz = params_.relocalization_resolution_xy; + in.output_lattice.resolution_yaw = + params_.relocalization_resolution_phi; + + // in.local_map: to be populated in running state // (shallow) copy metric maps into expected format: const auto& maps = state_.metric_map->maps; ASSERT_(!maps.empty()); + ASSERT_( + params_.metric_map_layer_names.empty() || + params_.metric_map_layer_names.size() == maps.size()); for (size_t i = 0; i < maps.size(); i++) - in.reference_map.layers[std::to_string(i)] = maps.at(i); + { + const std::string layerName = + params_.metric_map_layer_names.empty() + ? std::to_string(i) + : params_.metric_map_layer_names.at(i); + in.reference_map.layers[layerName] = maps.at(i); + } #else THROW_EXCEPTION("Should not reach here"); #endif @@ -759,21 +817,31 @@ void PFLocalizationCore::onStateRunning() if (auto& in = state_.pendingRelocalization->pending_se2; in) { // add missing field to "in": the input observation - in->observations += sf; + in->local_map.clear(); + + auto obs = + sf.getObservationByClass(0); + ASSERTMSG_( + obs, + "Relocalization assumes input SF has one CObservationPointCloud"); + ASSERT_(obs->pointcloud); + + in->local_map.layers["raw"] = obs->pointcloud; + + MRPT_LOG_INFO_STREAM("About to run Relocalization"); - const auto reloc = mola::RelocalizationLikelihood_SE2::run(*in); + const auto reloc = mola::RelocalizationICP_SE2::run(*in); MRPT_LOG_INFO_STREAM( "RelocalizationLikelihood_SE2 took " << reloc.time_cost << " s."); - auto candidates = mola::find_best_poses_se2( - reloc.likelihood_grid, params_.relocalization_best_percentile); - - ASSERT_(!candidates.empty()); + std::vector candidates; + reloc.found_poses.visitAllPoses( + [&](const auto& p) + { candidates.push_back(mrpt::math::TPose2D(p)); }); MRPT_LOG_INFO_STREAM( - "RelocalizationLikelihood_SE2 best candidate (out of " - << candidates.size() - << " top): " << candidates.rbegin()->second.asString()); + "RelocalizationLikelihood_SE2 gave " << candidates.size() + << " candidates"); // Create a few particles around each best candidate: ASSERT_(state_.pdf2d); @@ -787,7 +855,7 @@ void PFLocalizationCore::onStateRunning() 2, mrpt::round( params_.initial_particles_per_m2 * mrpt::square(sigmaXY))); - for (const auto& [score, pose] : candidates) + for (const auto& pose : candidates) { for (size_t i = 0; i < numCopies; i++) { @@ -922,15 +990,20 @@ void PFLocalizationCore::set_map_from_metric_map( // Convert it to CMultiMetricMap, and save the optional georeferrencing: auto mMap = mrpt::maps::CMultiMetricMap::Create(); + std::vector layerNames; for (const auto& [layerName, layerMap] : mm.layers) + { mMap->maps.push_back(layerMap); + layerNames.push_back(layerName); + } - this->set_map_from_metric_map(mMap, mm.georeferencing); + this->set_map_from_metric_map(mMap, mm.georeferencing, layerNames); } void PFLocalizationCore::set_map_from_metric_map( const mrpt::maps::CMultiMetricMap::Ptr& metricMap, - const std::optional& georeferencing) + const std::optional& georeferencing, + const std::vector& layerNames) { auto lck = mrpt::lockHelper(stateMtx_); @@ -954,6 +1027,7 @@ void PFLocalizationCore::set_map_from_metric_map( params_.metric_map = metricMap; params_.georeferencing = georeferencing; + params_.metric_map_layer_names = layerNames; // debug trace with full submap details: ---------------------------- MRPT_LOG_DEBUG_STREAM(