-
Notifications
You must be signed in to change notification settings - Fork 350
/
config_parameters.py
198 lines (162 loc) · 11.6 KB
/
config_parameters.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
"""
* This file is part of PYSLAM
*
* Copyright (C) 2016-present Luigi Freda <luigi dot freda at gmail dot com>
*
* PYSLAM is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* PYSLAM is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with PYSLAM. If not, see <http://www.gnu.org/licenses/>.
"""
# List of shared parameters for configuring SLAM modules
class Parameters:
# SLAM threads
kLocalMappingOnSeparateThread=True # True: move local mapping on a separate thread, False: tracking and then local mapping in a single thread
kTrackingWaitForLocalMappingToGetIdle=False # True: wait for local mapping to be idle before starting tracking, False: tracking and then local mapping in a single thread
kTrackingWaitForLocalMappingSleepTime=0.1 # DEPRECATED: -1 for no sleep # [s] (NOTE: a bit of sleep time increases the call rate of LBA and therefore VO accuracy)
kLocalMappingParallelKpsMatching=True
kLocalMappingParallelKpsMatchingNumWorkers=6
kLocalMappingDebugAndPrintToFile = True
# Number of desired keypoints per frame
kNumFeatures=2000
# Point triangulation
kCosMaxParallaxInitializer=0.99998 # 0.99998 # max cos angle for triangulation (min parallax angle) in the Initializer
kCosMaxParallax=0.9999 # 0.9998 # max cos angle for triangulation (min parallax angle)
kMinRatioBaselineDepth = 0.01
# Point visibility
kViewingCosLimitForPoint=0.5 # must be viewing cos < kViewingCosLimitForPoint (viewing angle must be less than 60 deg)
kScaleConsistencyFactor=1.5
kMaxDistanceToleranceFactor=1.2
kMinDistanceToleranceFactor=0.8
# Feature management
kSigmaLevel0 = 1.0 # sigma of the keypoint localization at level 0 (default value is 1 for ORB detector); can be changed by selected feature
kFeatureMatchRatioTest = 0.7 # TODO: put it in an table and make it configurable per descriptor
#kInitializerFeatureMatchRatioTest # ratio test used by Initializer
#
kKdtNmsRadius = 3 # pixels #3 # radius for kd-tree based Non-Maxima Suppression
#
kCheckFeaturesOrientation = True
# Initializer
kInitializerDesiredMedianDepth = 1 # when initializing, the initial median depth is computed and forced to this value (for better visualization is > 1)
kInitializerMinRatioDepthBaseline = 100 # compare to 1/kMinRatioBaselineDepth
kInitializerNumMinFeatures = 100
kInitializerNumMinFeaturesStereo = 500
kInitializerNumMinTriangulatedPoints = 150
kInitializerFeatureMatchRatioTest = 0.9 # ratio test used by Initializer # TODO: put it in an table and make it configurable per descriptor
kInitializerNumMinNumPointsForPnPWithDepth = 15
kInitializerUseCellCoverageCheck = True
kInitializerUseMinFrameDistanceCheck = True
# Tracking
kUseMotionModel = True # use or not the motion model for computing a first guess pose (that will be optimized by pose optimization)
kUseSearchFrameByProjection = True # match frames by using frame map points projection and epipolar lines; here, the current available interframe pose estimate is used for computing the fundamental mat F
kMinNumMatchedFeaturesSearchFrameByProjection=20 # if the number of tracked features is below this, then the search fails
kUseEssentialMatrixFitting = False # fit an essential matrix; orientation and keypoint match inliers are estimated by fitting an essential mat (5 points algorithm),
# WARNING: essential matrix fitting comes with some limitations (please, read the comments of the method slam.estimate_pose_ess_mat())
kMaxNumOfKeyframesInLocalMap = 80
kNumBestCovisibilityKeyFrames = 10
kUseVisualOdometryPoints = True
# Keyframe generation
kNumMinPointsForNewKf = 15 # minimum number of matched map points for spawning a new KeyFrame
kNumMinTrackedClosePointsForNewKfNonMonocular = 100 # minimum number of tracked close map points that for not spawning a new KeyFrame in case of a non-monocular system
kNumMaxNonTrackedClosePointsForNewKfNonMonocular = 70 # maximum number of non-tracked close map points for not spawning a new KeyFrame in case of a non-monocular system
kThNewKfRefRatio = 0.9 # for determining if a new KF must be spawned, condition 3
kThNewKfRefRatioStereo = 0.75 # for determining if a new KF must be spawned, condition 3, in the case non-monocular
kThNewKfRefRatioNonMonocualar = 0.25 # for determining if a new KF must be spawned in case the system is not monocular, condition 2b
kUseFeatureCoverageControlForNewKf = False # [experimental] check if all the matched map points in the current frame well cover the image (by using an image grid check)
# Keyframe culling
kKeyframeCullingRedundantObsRatio = 0.9
kKeyframeMaxTimeDistanceInSecForCulling = 0.5 # [s] # Use float('inf') for disabling it
kKeyframeCullingMinNumPoints = 50
# Stereo matching
kStereoMatchingMaxRowDistance = 1.1 # [pixels]
kStereoMatchingShowMatchedPoints = False # show the frame stereo matches (debug stereo matching)
# Search matches by projection
kMaxReprojectionDistanceFrame = 7 #7 # [pixels] o:7
kMaxReprojectionDistanceMap = 3 #3 # [pixels] o:1,(rgbd)3,(reloc)5 => mainly 2.5*th where th acts as a multiplicative factor
kMaxReprojectionDistanceMapRgbd = 5 #5 # [pixels] o:5
kMaxReprojectionDistanceFuse = 3 #3 # [pixels] o:3
kMaxReprojectionDistanceSim3 = 7.5 # [pixels] o:7.5
#
kMatchRatioTestMap=0.8
kMatchRatioTestEpipolarLine=0.8 # used just for test function find_matches_along_line()
#
# Reference max descriptor distance (used for initial checks and then updated and adapted)
kMaxDescriptorDistance=0 # it is initialized by the first created instance of feature_manager.py at runtime
# Search matches for triangulation by using epipolar lines
kMinDistanceFromEpipole=10 # [pixels] Used with search by epipolar lines
# Local Mapping
kLocalMappingNumNeighborKeyFrames=20 # [# frames] for generating new points and fusing them
kLocalMappingTimeoutPopKeyframe=0.5 # [s]
# Covisibility graph
kMinNumOfCovisiblePointsForCreatingConnection=15
# Sparse map visualization
kSparseImageColorPatchDelta=1 # center +- delta
# Bundle Adjustment (BA)
kLocalBAWindow=20 # [# frames]
kUseLargeWindowBA=False # True: perform BA over a large window; False: do not perform large window BA
kEveryNumFramesLargeWindowBA=10 # num of frames between two large window BA
kLargeBAWindow=20 # [# frames]
kUseParallelProcessLBA = False # Experimental - keep it False for the moment since it is neither faster (probably due to the overhead of copying data to multi-processing shared memory) nor stable yet!
# Loop closing
kUseLoopClosing = True # To enable/disable loop closing.
kMinDeltaFrameForMeaningfulLoopClosure = 10
kMaxResultsForLoopClosure = 5
kLoopDetectingTimeoutPopKeyframe=0.5 # [s]
kLoopClosingDebugWithLoopDetectionImages = False
kLoopClosingDebugWithSimmetryMatrix = True
kLoopClosingDebugAndPrintToFile = True
kLoopClosingDebugWithLoopConsistencyCheckImages = True
kLoopClosingDebugShowLoopMatchedPoints = False
kLoopClosingParallelKpsMatching=True
kLoopClosingParallelKpsMatchingNumWorkers = 6
kLoopClosingGeometryCheckerMinKpsMatches = 20 # o:20
kLoopClosingTh2 = 10
kLoopClosingMaxReprojectionDistanceMapSearch = 10 # [pixels] o:10
kLoopClosingMinNumMatchedMapPoints = 40
kLoopClosingMaxReprojectionDistanceFuse = 4 # [pixels] o:4
kLoopClosingFeatureMatchRatioTest = 0.75 # TODO: put it in an table and make it configurable per descriptor
# Relocatization
kRelocalizationDebugAndPrintToFile = False
kRelocalizationMinKpsMatches = 15 # o:15
kRelocalizationParallelKpsMatching=True
kRelocalizationParallelKpsMatchingNumWorkers = 6
kRelocalizationFeatureMatchRatioTest = 0.75 # TODO: put it in an table and make it configurable per descriptor
kRelocalizationFeatureMatchRatioTestLarge = 0.9 # o:0.9
kRelocalizationPoseOpt1MinMatches = 10 # o:10
kRelocalizationDoPoseOpt2NumInliers = 50 # o:50
kRelocalizationMaxReprojectionDistanceMapSearchCoarse = 10 # [pixels] o:10
kRelocalizationMaxReprojectionDistanceMapSearchFine = 3 # [pixels] o:3
# Global Bundle Adjustment (GBA)
kUseGBA = True # Activated by loop closing
kGBADebugAndPrintToFile = True
kGBAUseRobustKernel = True
# Volume Integration
kUseVolumetricIntegration = False # To enable/disable volumetric integration (dense mapping)
kVolumetricIntegrationDebugAndPrintToFile = True
kVolumetricIntegrationExtractMesh = False # Extract mesh or point cloud as output
kVolumetricIntegrationVoxelLength = 0.015 # [m]
kVolumetricIntegrationSdfTrunc = 0.04 # [m]
kVolumetricIntegrationDepthTruncIndoor = 4.0 # [m]
kVolumetricIntegrationDepthTruncOutdoor = 10.0 # [m]
kVolumetricIntegrationMinNumLBATimes = 1 # We integrate only the keyframes that have been processed by LBA at least kVolumetricIntegrationMinNumLBATimes times.
kVolumetricIntegrationOutputTimeInterval = 1.0 # [s]
kVolumetricIntegrationUseDepthEstimator = False # Use depth estimator for volumetric integration in the back-end.
# Since the depth inference time is above 1 second, this is very slow.
# NOTE: the depth estimator estimates a metric depth (with an absolute scale). You can't combine it with a MONOCULAR SLAM since the SLAM map scale will be not consistent.
kVolumetricIntegrationDepthEstimatorType = "DEPTH_RAFT_STEREO" # "DEPTH_PRO","DEPTH_ANYTHING_V2, "DEPTH_SGBM", "DEPTH_RAFT_STEREO", "DEPTH_CRESTEREO_PYTORCH" (see depth_estimator_factory.py)
kVolumetricIntegrationDepthEstimationFilterShadowPoints = True
# Depth estimator (experimental usage in the front-end, WIP)
kUseDepthEstimatorInFrontEnd = False # To enable/disable depth estimation with monocular front-end.
# You can directly set your desired depth estimator in main_slam.py.
kDepthEstimatorRemoveShadowPointsInFrontEnd = True
# other parameters
kChi2Mono = 5.991 # chi-square 2 DOFs, used for reprojection error (Hartley Zisserman pg 119)
kChi2Stereo = 7.815 # chi-square 3 DOFs, used for reprojection error (Hartley Zisserman pg 119)