Skip to content

Commit

Permalink
WIP: use icp relocalization
Browse files Browse the repository at this point in the history
  • Loading branch information
@jlblancoc
jlblancoc committed Apr 27, 2024
1 parent 47a4781 commit 97a9755
Show file tree
Hide file tree
Showing 4 changed files with 177 additions and 28 deletions.
2 changes: 2 additions & 0 deletions mrpt_pf_localization/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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
Expand Down
12 changes: 11 additions & 1 deletion mrpt_pf_localization/include/mrpt_pf_localization/mrpt_pf_localization_core.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,10 @@

#pragma once

#include <mp2p_icp/ICP.h>
#include <mp2p_icp/Parameters.h>
#include <mp2p_icp/metricmap.h>
#include <mp2p_icp_filters/FilterBase.h>
#include <mrpt/bayes/CParticleFilter.h>
#include <mrpt/containers/yaml.h>
#include <mrpt/core/Clock.h>
Expand Down Expand Up @@ -60,6 +63,7 @@ class PFLocalizationCore : public mrpt::system::COutputLogger

mrpt::maps::CMultiMetricMap::Ptr metric_map; //!< Empty=uninitialized
std::optional<mp2p_icp::metric_map_t::Georeferencing> georeferencing;
std::vector<std::string> metric_map_layer_names;

/** Shows a custom MRPT GUI with the PF and map state
* Can be changed at any moment.
Expand Down Expand Up @@ -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):
Expand Down Expand Up @@ -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<mp2p_icp::metric_map_t::Georeferencing>&
georeferencing = std::nullopt);
georeferencing = std::nullopt,
const std::vector<std::string>& layerNames = {});

/** Defines the map to use from a metric_map_t map, with optional
* georeferencing.
Expand Down
79 changes: 71 additions & 8 deletions mrpt_pf_localization/params/default.config.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -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
112 changes: 93 additions & 19 deletions mrpt_pf_localization/src/mrpt_pf_localization/mrpt_pf_localization_core.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,13 +6,15 @@
| All rights reserved. Released under BSD 3-Clause license. See LICENSE |
+------------------------------------------------------------------------+ */

#include <mp2p_icp/icp_pipeline_from_yaml.h>
#include <mrpt/config/CConfigFile.h>
#include <mrpt/config/CConfigFileMemory.h>
#include <mrpt/core/lock_helper.h>
#include <mrpt/maps/CLandmarksMap.h>
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/obs/CActionCollection.h>
#include <mrpt/obs/CObservationPointCloud.h>
#include <mrpt/opengl/CEllipsoid2D.h>
#include <mrpt/opengl/CEllipsoid3D.h>
#include <mrpt/opengl/CPointCloud.h>
Expand Down Expand Up @@ -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<mola::RelocalizationLikelihood_SE2::Input> pending_se2;
std::optional<mola::RelocalizationICP_SE2::Input> pending_se2;
#endif
};

Expand Down Expand Up @@ -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<double>(0.5, pDiag.norm() / 3);
in.initial_guess_lattice.resolution_phi =
std::max<double>(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
Expand Down Expand Up @@ -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<mrpt::obs::CObservationPointCloud>(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<mrpt::math::TPose2D> 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);
Expand All @@ -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++)
{
Expand Down Expand Up @@ -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<std::string> 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<mp2p_icp::metric_map_t::Georeferencing>& georeferencing)
const std::optional<mp2p_icp::metric_map_t::Georeferencing>& georeferencing,
const std::vector<std::string>& layerNames)
{
auto lck = mrpt::lockHelper(stateMtx_);

Expand All @@ -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(
Expand Down

0 comments on commit 97a9755

Please sign in to comment.