Skip to content

Commit

Permalink
Add a demo ICP-slam with voxelmap
Browse files Browse the repository at this point in the history
  • Loading branch information
jlblancoc committed Oct 17, 2023
1 parent fa935bc commit b13497d
Show file tree
Hide file tree
Showing 3 changed files with 120 additions and 1 deletion.
4 changes: 4 additions & 0 deletions libs/maps/include/mrpt/maps/CVoxelMap.h
Original file line number Diff line number Diff line change
Expand Up @@ -81,6 +81,8 @@ class CVoxelMap : public CVoxelMapBase<int8_t>,
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);
Expand All @@ -101,6 +103,8 @@ class CVoxelMap : public CVoxelMapBase<int8_t>,
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);
Expand Down
18 changes: 17 additions & 1 deletion libs/maps/src/maps/CVoxelMap.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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(
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
99 changes: 99 additions & 0 deletions share/mrpt/config_files/icp-slam/icp-slam_demo_voxelmap.ini
Original file line number Diff line number Diff line change
@@ -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

0 comments on commit b13497d

Please sign in to comment.