From b13497d2a827486555ff6e900d9784b0de777135 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Tue, 17 Oct 2023 12:47:54 +0200 Subject: [PATCH] Add a demo ICP-slam with voxelmap --- libs/maps/include/mrpt/maps/CVoxelMap.h | 4 + libs/maps/src/maps/CVoxelMap.cpp | 18 +++- .../icp-slam/icp-slam_demo_voxelmap.ini | 99 +++++++++++++++++++ 3 files changed, 120 insertions(+), 1 deletion(-) create mode 100644 share/mrpt/config_files/icp-slam/icp-slam_demo_voxelmap.ini diff --git a/libs/maps/include/mrpt/maps/CVoxelMap.h b/libs/maps/include/mrpt/maps/CVoxelMap.h index 44147b69ab..c720f75b9d 100644 --- a/libs/maps/include/mrpt/maps/CVoxelMap.h +++ b/libs/maps/include/mrpt/maps/CVoxelMap.h @@ -81,6 +81,8 @@ class CVoxelMap : public CVoxelMapBase, void loadFromConfigFile( const mrpt::config::CConfigFileBase& source, const std::string& section) override; + void saveToConfigFile( + mrpt::config::CConfigFileBase& c, const std::string& s) const; void writeToStream(mrpt::serialization::CArchive& out) const; void readFromStream(mrpt::serialization::CArchive& in); @@ -101,6 +103,8 @@ class CVoxelMap : public CVoxelMapBase, void loadFromConfigFile( const mrpt::config::CConfigFileBase& source, const std::string& section) override; + void saveToConfigFile( + mrpt::config::CConfigFileBase& c, const std::string& s) const; void writeToStream(mrpt::serialization::CArchive& out) const; void readFromStream(mrpt::serialization::CArchive& in); diff --git a/libs/maps/src/maps/CVoxelMap.cpp b/libs/maps/src/maps/CVoxelMap.cpp index 2ef93db103..a4435b278f 100644 --- a/libs/maps/src/maps/CVoxelMap.cpp +++ b/libs/maps/src/maps/CVoxelMap.cpp @@ -16,7 +16,7 @@ using namespace mrpt::maps; using namespace std::string_literals; // "..."s // =========== Begin of Map definition ============ -MAP_DEFINITION_REGISTER("mrpt::maps::CVoxelMap", mrpt::maps::CVoxelMap) +MAP_DEFINITION_REGISTER("mrpt::maps::CVoxelMap,voxelMap", mrpt::maps::CVoxelMap) CVoxelMap::TMapDefinition::TMapDefinition() = default; void CVoxelMap::TMapDefinition::loadFromConfigFile_map_specific( @@ -264,6 +264,17 @@ void CVoxelMap::TInsertionOptions::loadFromConfigFile( MRPT_LOAD_CONFIG_VAR(ray_trace_free_space, bool, c, s); MRPT_LOAD_CONFIG_VAR(decimation, uint64_t, c, s); } +void CVoxelMap::TInsertionOptions::saveToConfigFile( + mrpt::config::CConfigFileBase& c, const std::string& s) const +{ + MRPT_SAVE_CONFIG_VAR(max_range, c, s); + MRPT_SAVE_CONFIG_VAR(prob_miss, c, s); + MRPT_SAVE_CONFIG_VAR(prob_hit, c, s); + MRPT_SAVE_CONFIG_VAR(clamp_min, c, s); + MRPT_SAVE_CONFIG_VAR(clamp_max, c, s); + MRPT_SAVE_CONFIG_VAR(ray_trace_free_space, c, s); + MRPT_SAVE_CONFIG_VAR(decimation, c, s); +} void CVoxelMap::TInsertionOptions::writeToStream( mrpt::serialization::CArchive& out) const @@ -318,6 +329,11 @@ void CVoxelMap::TLikelihoodOptions::loadFromConfigFile( { MRPT_LOAD_CONFIG_VAR(decimation, int, c, s); } +void CVoxelMap::TLikelihoodOptions::saveToConfigFile( + mrpt::config::CConfigFileBase& c, const std::string& s) const +{ + MRPT_SAVE_CONFIG_VAR(decimation, c, s); +} void CVoxelMap::TLikelihoodOptions::writeToStream( mrpt::serialization::CArchive& out) const diff --git a/share/mrpt/config_files/icp-slam/icp-slam_demo_voxelmap.ini b/share/mrpt/config_files/icp-slam/icp-slam_demo_voxelmap.ini new file mode 100644 index 0000000000..ceb318e8b1 --- /dev/null +++ b/share/mrpt/config_files/icp-slam/icp-slam_demo_voxelmap.ini @@ -0,0 +1,99 @@ +#------------------------------------------------------------ +# Config file for the "ICP-SLAM" application +# See: https://www.mrpt.org/list-of-mrpt-apps/application-icp-slam/ +#------------------------------------------------------------ + +#======================================================= +# Section: [ICP] +# Parameters of ICP inside the ICP-based SLAM class +#======================================================= +[ICP] +maxIterations = 80 // The maximum number of iterations to execute if convergence is not achieved before +minAbsStep_trans = 1e-6 // If the correction in all translation coordinates (X,Y,Z) is below this threshold (in meters), iterations are terminated: +minAbsStep_rot = 1e-6 // If the correction in all rotation coordinates (yaw,pitch,roll) is below this threshold (in radians), iterations are terminated: + +thresholdDist = 0.3 // Initial maximum distance for matching a pair of points +thresholdAng_DEG = 5 // An angular factor (in degrees) to increase the matching distance for distant points. + +ALFA = 0.8 // After convergence, the thresholds are multiplied by this constant and ICP keep running (provides finer matching) + +smallestThresholdDist=0.05 // This is the smallest the distance threshold can become after stopping ICP and accepting the result. +onlyClosestCorrespondences=true // 1: Use the closest points only, 0: Use all the correspondences within the threshold (more robust sometimes, but slower) +onlyUniqueRobust = true // Force unique correspondences in both directions when pairing two point clouds + +# 0: icpClassic +# 1: icpLevenbergMarquardt +ICP_algorithm = icpClassic + +# decimation to apply to the point cloud being registered against the map +# Reduce to "1" to obtain the best accuracy +corresponding_points_decimation = 5 + +#======================================================= +# Section: [MappingApplication] +# Use: Here comes global parameters for the app. +#======================================================= +[MappingApplication] +# The source file (RAW-LOG) with action/observation pairs +rawlog_file="../../datasets/2006-01ENE-21-SENA_Telecom Faculty_one_loop_only.rawlog" +rawlog_offset=0 + +# The directory where the log files will be saved (left in blank if no log is required) +logOutput_dir=LOG_ICP-SLAM +LOG_FREQUENCY=50 // The frequency of log files generation: +SAVE_3D_SCENE=1 +SAVE_POSE_LOG=0 +CAMERA_3DSCENE_FOLLOWS_ROBOT=1 +SHOW_PROGRESS_3D_REAL_TIME=1 + +SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS=5 +SHOW_LASER_SCANS_3D = true + +localizationLinDistance = 0.10 // The distance threshold for correcting odometry with ICP (meters) +localizationAngDistance = 5 // The distance threshold for correcting odometry with ICP (degrees) + +insertionLinDistance = 1.0 // The distance threshold for inserting observations in the map (meters) +insertionAngDistance = 15.0 // The distance threshold for inserting observations in the map (degrees) + +minICPgoodnessToAccept = 0.20 // Minimum ICP quality to accept correction [0,1]. + +matchAgainstTheGrid = false + +# ======================================================== +# MULTIMETRIC MAP CONFIGURATION +# See docs for (Google for) mrpt::maps::CMultiMetricMap +# ======================================================== +# Creation of maps: +voxelMap_count = 1 +pointsMap_count = 1 + +# ==================================================== +# MULTIMETRIC MAP: PointsMap #00 +# ==================================================== +# Creation Options for PointsMap 00: +[MappingApplication_pointsMap_00_insertOpts] +minDistBetweenLaserPoints = 0.04 +fuseWithExisting = false +isPlanarMap = true + +# ==================================================== +# MULTIMETRIC MAP: VoxelMap #00 +# ==================================================== +# Creation Options for VoxelMap #00: +[MappingApplication_voxelMap_00_creationOpts] +resolution = 0.05 // (meters) + +# Insertion Options for VoxelMap #00: +[MappingApplication_voxelMap_00_insertOpts] +max_range = -1 // (Default=-1, no limits) +prob_hit = 0.7 // sets the probablility for a "hit" (will be converted to logodds) - sensor model +prob_miss = 0.4 // sets the probablility for a "miss" (will be converted to logodds) - sensor model +clamp_min = 0.05 // sets the minimum threshold for occupancy clamping (sensor model) +clamp_max = 0.95 // sets the maximum threshold for occupancy clamping (sensor model) +ray_trace_free_space = false +decimation = 1 + +# Likelihood Options for VoxelMap #00: +[MappingApplication_voxelMap_00_likelihoodOpts] +# None +