#------------------------------------------------------------
# Config file for the "RBPF-SLAM" application
# See: https://www.mrpt.org/list-of-mrpt-apps/application-rbpf-slam/
#------------------------------------------------------------
# Optimal sampling algorithm: 3: pfAuxiliaryPFOptimal


#=======================================================
# 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_MALAGA_RBPF_GRIDMAPPING_ICP_LevMar
LOG_FREQUENCY=10			// The frequency of log files generation:
SAVE_MAP_IMAGES=1
SAVE_3D_SCENE=1
SAVE_POSE_LOG=0

CAMERA_3DSCENE_FOLLOWS_ROBOT=0
SHOW_PROGRESS_IN_WINDOW=1

insertionLinDistance     = 1.20	  // The distance threshold for inserting observations in the map (meters)
insertionAngDistance_deg = 35.0	  // The distance threshold for inserting observations in the map (degrees)

localizeLinDistance		 = 1.20	  // The distance threshold for updating the RBPF
localizeAngDistance_deg  = 35.0	  // The distance threshold for inserting observations in the map (degrees)

# Console verbosity level: 
# One of: DEBUG, INFO (default), WARN, ERROR
verbosity_level = DEBUG

#----------------------------------------------------------------------------------
# The Particle Filter algorithm:
#	0: pfStandardProposal
#	1: pfAuxiliaryPFStandard
#	2: pfOptimalProposal      *** (ICP-based (Grisetti method),...)
#	3: pfAuxiliaryPFOptimal	  *** (Optimal SAMPLING)
#
# See: http://babel.isa.uma.es/mrpt/index.php/Particle_Filter_Algorithms
#----------------------------------------------------------------------------------
PF_algorithm		= 2
adaptiveSampleSize	= 0		// 0: Fixed # of particles, 1: KLD adaptive

#----------------------------------------------------------------------------------
# The Particle Filter Resampling method:
#	0: prMultinomial
#	1: prResidual
#	2: prStratified
#	3: prSystematic
#
# See: http://babel.isa.uma.es/mrpt/index.php/Resampling_Schemes
#----------------------------------------------------------------------------------
resamplingMethod=0

sampleSize=10			// Sample size (for fixed number)
BETA=0.50			// Resampling ESS threshold


# ========================================================
#            MULTIMETRIC MAP CONFIGURATION
# See docs for (Google for) mrpt::maps::CMultiMetricMap
# ========================================================
# Creation of maps:
occupancyGrid_count  = 0
gasGrid_count        = 0
beaconMap_count      = 0
pointsMap_count      = 1

# Selection of map for likelihood: (fuseAll=-1,occGrid=0, points=1,landmarks=2,gasGrid=3)
likelihoodMapSelection=-1


# ====================================================
#         CHybridMetricMapPDF::TPredictionParams
# ====================================================
#powFactor=0.01					// A "power factor" for updating weights
pfAuxFilterOptimal_MaximumSearchSamples=250	// For PF algorithm=3


# -----------------------------------------------------------------
#   pfOptimalProposal_mapSelection
#  Only for PF algorithm=2 (Exact "pfOptimalProposal")
#   Select the map on which to calculate the optimal 
#    proposal distribution. Values:
#   0: Gridmap   -> Uses Scan matching-based approximation (based on Stachniss work)
#   1: Landmarks -> Uses matching to approximate optimal
#   2: Beacons   -> Used for exact optimal proposal in RO-SLAM
#   3: Points-map-> Uses Scan matching-based approximation (based on Stachniss work)
# -----------------------------------------------------------------
pfOptimalProposal_mapSelection=3

# Adaptive sample size parameters ------------------
KLD_maxSampleSize=150
KLD_minSampleSize=15
KLD_binSize_XY=0.03
KLD_binSize_PHI_deg=1
KLD_delta=0.001
KLD_epsilon=0.5


# ====================================================
#         ICP configuration.
#      Only used if PF_algorithm=2
# ====================================================
# 0: icpClassic
# 1: icpLevenbergMarquardt
# 2: icpIKF
ICP_algorithm    = 0
maxIterations    = 60 		// The maximum number of iterations to execute if convergence is not achieved before
thresholdDist    = 1.00		// 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.50 			// After convergence, the thresholds are multiplied by this constant and ICP keep running (provides finer matching)
smallestThresholdDist      = 0.10// This is the smallest the distance threshold can become after stopping ICP and accepting the result.
onlyClosestCorrespondences = 1	// 1: Use the closest points only, 0: Use all the correspondences within the threshold 

# ====================================================
#   MULTIMETRIC MAP: PointsMap #00
# ====================================================
# Likelihood Options for Pointsmap 00:
[MappingApplication_pointsMap_00_likelihoodOpts]
max_corr_distance	= 1.0
sigma_dist			= 0.05