RBPF-SLAM算法（C ++库mrpt-slam）-AGV吧

1.数学背景
阅读资料：

“动态贝叶斯网络的Rao-Blackwellised粒子滤波”，Doucet，A。和De Freitas，N。和Murphy，K。和Russell，S。（2000） – PDF
Boris Lipchin的一些幻灯片。

2.在MRPT中实施基于RBPF的SLAM解决方案
RBPF是粒子滤波器的一个特殊实例，因此原则上可以使用虚拟C ++基类中声明的四种通用滤波算法中的任何一种来顺序估计机器人路径。只是对算法的简要总结：

序贯重要性重采样 – SIR与“标准”的建议分布（枚举值： pfStandardProposal）
辅助粒子滤波 -与“标准”建议分布APF（枚举值： pfAuxiliaryPFStandard）
最佳建议分布（枚举值： pfOptimalProposal）
近似最优采样 – A-基于采样排斥逼近最佳建议时的闭合形式的表达不可用（枚举值： pfAuxiliaryPFOptimal）
注意：到目前为止（2011年8月），APF算法没有实现RBPF-SLAM（这反过来对Monte Carlo本地化有用）。

2.1。 pfStandardProposal：带有“标准提案”的SIR和任何类型的指标图
描述：只要在每张地图上为传感器观测实现观测似然函数，该 pfStandardProposal 算法可以与任何度量地图或其中几个地图的组合一起使用。然而，这种算法并不值得推荐，因为“标准”提案分布是运动模型（“ 动作 ”），并且大多数粒子将以与观测不相容的区域结束，因此重采样将经常发生。只有在已知运动模型非常精确的情况下才使用该算法。配置文件示例：

2.2。 pfAuxiliaryPFOptimal：使用任何类型的公制映射的近似最优建议
说明：只要在每张地图中为传感器观测实现观测似然函数， pfAuxiliaryPFOptimal 算法（本文和本文中介绍的）可以与任何度量地图或其中几个地图的组合一起使用。无论运动模型的质量如何，算法都会自动逼近最优投标分布。然而，运动模型越差，运行每次迭代所需的时间就越长。配置文件示例：

[code]

#------------------------------------------------------------
# Config file for the "RBPF-SLAM" application
# See: http://www.mrpt.org/list-of-mrpt-apps/application-rbpf-slam/
#------------------------------------------------------------


#=======================================================
# 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_GRIDMAPPING
LOG_FREQUENCY=10			// The frequency of log files generation:
GENERATE_LOG_JOINT_H=0
GENERATE_LOG_INFO=0
SAVE_MAP_IMAGES=1
SAVE_3D_SCENE=1
SAVE_POSE_LOG=0
SAVE_ENOSE_READINGS=0
CAMERA_3DSCENE_FOLLOWS_ROBOT=0
SHOW_PROGRESS_IN_WINDOW=1

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

localizeLinDistance     = 1.0	// The distance threshold for actually running the PF, otherwise will rely on odometry  (meters)
localizeAngDistance_deg = 40.0	// The rotational threshold for actually running the PF, otherwise will rely on odometry  (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's method),...)
#	3: pfAuxiliaryPFOptimal	  *** (Optimal SAMPLING)
#
# See: http://babel.isa.uma.es/mrpt/index.php/Particle_Filter_Algorithms
#----------------------------------------------------------------------------------
PF_algorithm		= 3
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=25			// 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=1
gasGrid_count=0
landmarksMap_count=0
beaconMap_count=0
pointsMap_count=0

# 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=400	// 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
# -----------------------------------------------------------------
pfOptimalProposal_mapSelection=0

# 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


# ====================================================
#   MULTIMETRIC MAP: OccGrid #00
# ====================================================
# Creation Options for OccupancyGridMap 00:
[MappingApplication_occupancyGrid_00_creationOpts]
resolution=0.07
disableSaveAs3DObject=0


# Insertion Options for OccupancyGridMap 00:
[MappingApplication_occupancyGrid_00_insertOpts]
mapAltitude					= 0
useMapAltitude				= 0
maxDistanceInsertion		= 35
maxOccupancyUpdateCertainty	= 0.60
considerInvalidRangesAsFreeSpace = 1
minLaserScanNoiseStd		= 0.001
wideningBeamsWithDistance 	= 0


# Likelihood Options for OccupancyGridMap 00:
[MappingApplication_occupancyGrid_00_likelihoodOpts]
likelihoodMethod=4		// 0=MI, 1=Beam Model, 2=RSLC, 3=Cells Difs, 4=LF_Trun, 5=LF_II
LF_decimation=5
LF_stdHit=0.20
LF_maxCorrsDistance=0.30
LF_zHit=0.999
LF_zRandom=0.001
LF_maxRange=30
LF_alternateAverageMethod=0

enableLikelihoodCache=1

[/code]

使用一台SICK激光扫描仪和一个中型（约100米长）环路封闭的网格映射示例。
[code]

#------------------------------------------------------------
# Config file for the "RBPF-SLAM" application
# See: http://www.mrpt.org/list-of-mrpt-apps/application-rbpf-slam/
#------------------------------------------------------------


#=======================================================
# Section: [MappingApplication]
# Use: Here comes global parameters for the app.
#=======================================================
[MappingApplication]
# The source file (RAW-LOG) with action/observation pairs
#rawlog_file=../datasets/2007-05MAY-17_SENA_Floor2.3_3lasers.rawlog
rawlog_file=J:/Trabajo/RAWLogs/2007-05MAY-17_SENA_Floor2.3_3lasers.rawlog
rawlog_offset=0

# The directory where the log files will be saved (left in blank if no log is required)
logOutput_dir=LOG_3GRIDMAPS
LOG_FREQUENCY=5			// The frequency of log files generation:
GENERATE_LOG_JOINT_H=0
GENERATE_LOG_INFO=0
SAVE_MAP_IMAGES=1
SAVE_3D_SCENE=1
SAVE_POSE_LOG=0
SAVE_ENOSE_READINGS=0
CAMERA_3DSCENE_FOLLOWS_ROBOT=0
SHOW_PROGRESS_IN_WINDOW=1

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

localizeLinDistance     = 0.1	// The distance threshold for actually running the PF, otherwise will rely on odometry  (meters)
localizeAngDistance_deg = 4.0	// The rotational threshold for actually running the PF, otherwise will rely on odometry  (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,...)
#	3: pfAuxiliaryPFOptimal	  *** (Optimal Aux)
#
# See: /docs/html/topic_pf.html  or http://mrpt.sourceforge.net/topic_pf.html
#----------------------------------------------------------------------------------
PF_algorithm=3
adaptiveSampleSize=0		// 0: Fixed # of particles, 1: KLD adaptive

#----------------------------------------------------------------------------------
# The Particle Filter Resampling method:
#	0: prMultinomial
#	1: prResidual
#	2: prStratified
#	3: prSystematic
#
# See: /docs/html/topic_resampling.html or http://mrpt.sourceforge.net/topic_resampling.html
#----------------------------------------------------------------------------------
resamplingMethod=3

sampleSize=15			// 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=3
gasGrid_count=0
landmarksMap_count=0
beaconMap_count=0
pointsMap_count=0

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

# Enables (1) / Disables (0) insertion into specific maps:
enableInsertion_pointsMap=1
enableInsertion_landmarksMap=1
enableInsertion_beaconMap=1
enableInsertion_gridMaps=1
enableInsertion_gasGridMaps=1

# ====================================================
#         CHybridMetricMapPDF::TPredictionParams
# ====================================================
powFactor=0.05					// A "power factor" for updating weights
pfAuxFilterOptimal_MaximumSearchSamples=200	// 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
# -----------------------------------------------------------------
pfOptimalProposal_mapSelection=2


# Adaptive sample size parameters ------------------
KLD_maxSampleSize=10000
KLD_minSampleSize=15
KLD_binSize_XY=10.00
KLD_binSize_PHI_deg=30
KLD_delta=0.01
KLD_epsilon=0.40


# ====================================================
#   MULTIMETRIC MAP: OccGrid #00
# ====================================================
# Creation Options for OccupancyGridMap 00:
[MappingApplication_occupancyGrid_00_creationOpts]
resolution=0.05
disableSaveAs3DObject=0


# Insertion Options for OccupancyGridMap 00:
[MappingApplication_occupancyGrid_00_insertOpts]
mapAltitude=0.30
useMapAltitude=1
maxDistanceInsertion=40
maxOccupancyUpdateCertainty=0.54
considerInvalidRangesAsFreeSpace=1
minLaserScanNoiseStd=0.001


# Likelihood Options for OccupancyGridMap 00:
[MappingApplication_occupancyGrid_00_likelihoodOpts]
likelihoodMethod=4		// 0=MI, 1=Beam Model, 2=RSLC, 3=Cells Difs, 4=LF_Trun, 5=LF_II
LF_decimation=5
LF_stdHit=0.10
LF_maxCorrsDistance=0.30
LF_zHit=0.999
LF_zRandom=0.001
LF_maxRange=30
LF_alternateAverageMethod=0


# Creation Options for OccupancyGridMap 01:
[MappingApplication_occupancyGrid_01_creationOpts]
resolution=0.05
disableSaveAs3DObject=0


# Insertion Options for OccupancyGridMap 01:
[MappingApplication_occupancyGrid_01_insertOpts]
mapAltitude=0.51
useMapAltitude=1
maxDistanceInsertion=40
maxOccupancyUpdateCertainty=0.54
considerInvalidRangesAsFreeSpace=1
minLaserScanNoiseStd=0.001


# Likelihood Options for OccupancyGridMap 01:
[MappingApplication_occupancyGrid_01_likelihoodOpts]
likelihoodMethod=4		// 0=MI, 1=Beam Model, 2=RSLC, 3=Cells Difs, 4=LF_Trun, 5=LF_II
LF_decimation=5
LF_stdHit=0.10
LF_maxCorrsDistance=0.30
LF_zHit=0.999
LF_zRandom=0.001
LF_maxRange=30
LF_alternateAverageMethod=0


# Creation Options for OccupancyGridMap 02:
[MappingApplication_occupancyGrid_02_creationOpts]
resolution=0.05
disableSaveAs3DObject=0


# Insertion Options for OccupancyGridMap 02:
[MappingApplication_occupancyGrid_02_insertOpts]
mapAltitude=1.47
useMapAltitude=1
maxDistanceInsertion=40
maxOccupancyUpdateCertainty=0.54
considerInvalidRangesAsFreeSpace=1
minLaserScanNoiseStd=0.001


# Likelihood Options for OccupancyGridMap 02:
[MappingApplication_occupancyGrid_02_likelihoodOpts]
likelihoodMethod=4		// 0=MI, 1=Beam Model, 2=RSLC, 3=Cells Difs, 4=LF_Trun, 5=LF_II
LF_decimation=5
LF_stdHit=0.10
LF_maxCorrsDistance=0.30
LF_zHit=0.999
LF_zRandom=0.001
LF_maxRange=30
LF_alternateAverageMethod=0

[/code]

三个不同高度的水平激光扫描仪的三个网格地图。

使用RBPF进行网格映射的示例（rbpf-slam应用程序）

2.3。 pfOptimalProposal：网格地图的最佳建议
说明：该 pfOptimalProposal 算法为栅格图对应于由G. Grisetti等人介绍了最佳提案的启发式近似。在：“用Rao-Blackwellized粒子滤波器进行网格映射的改进技术”，Grisetti，G. Stachniss，C. Burgard，W. IEEE TRANSACTIONS ON ROBOTICS（2007） – PDF MRPT中的实现依赖于版本的ICP格网格。另一种实现（独立于MRPT）是流行的 gmapping。注意： pfOptimalProposal 需要设置参数 pfOptimalProposal_mapSelection （有关详细信息，请参阅示例配置文件）。配置文件示例：

[code]

#------------------------------------------------------------
# Config file for the "RBPF-SLAM" application
# See: http://www.mrpt.org/list-of-mrpt-apps/application-rbpf-slam/
#------------------------------------------------------------


#=======================================================
# 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)

#----------------------------------------------------------------------------------
# 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  = 1
gasGrid_count        = 0
landmarksMap_count   = 0
beaconMap_count      = 0
pointsMap_count      = 0

# 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=0

# 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: OccGrid #00
# ====================================================
# Creation Options for OccupancyGridMap 00:
[MappingApplication_occupancyGrid_00_creationOpts]
resolution=0.07
disableSaveAs3DObject=0


# Insertion Options for OccupancyGridMap 00:
[MappingApplication_occupancyGrid_00_insertOpts]
mapAltitude					= 0
useMapAltitude				= 0
maxDistanceInsertion		= 35
maxOccupancyUpdateCertainty	= 0.60
considerInvalidRangesAsFreeSpace = 1
minLaserScanNoiseStd		= 0.001
wideningBeamsWithDistance 	= 0


# Likelihood Options for OccupancyGridMap 00:
[MappingApplication_occupancyGrid_00_likelihoodOpts]
likelihoodMethod=4		// 0=MI, 1=Beam Model, 2=RSLC, 3=Cells Difs, 4=LF_Trun, 5=LF_II
LF_decimation=5
LF_stdHit=0.50
LF_maxCorrsDistance=0.30
LF_zHit=0.999
LF_zRandom=0.001
LF_maxRange=30
LF_alternateAverageMethod=0

enableLikelihoodCache=1

[/code]

使用中等大小的闭环网格进行网格映射。
2.4。 pfOptimalProposal：点图的最佳建议
说明：该 pfOptimalProposal 算法点地图是Grisetti等人的想法的一个版本，而是适用于点云（更快的更新比栅格图）。AFAIK，这种方法尚未在文献中发表，但工作很好。注意： pfOptimalProposal 需要设置参数 pfOptimalProposal_mapSelection （有关详细信息，请参阅示例配置文件）。配置文件示例：

[code]

#------------------------------------------------------------
# Config file for the "RBPF-SLAM" application
# See: http://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
landmarksMap_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

[/code]

用中等尺寸环路闭合点云映射。

1. 本网站的文章部分内容可能来源于网络，仅供大家学习与参考，如有侵权，请联系站长 QQ2766242327进行删除处理。
2. 本站资源大多存储在云盘，如发现链接失效，请联系我们我们会第一时间更新。

THE END