Rtabmap.h
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #ifndef RTABMAP_H_
29 #define RTABMAP_H_
30 
31 #include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
32 
36 #include "rtabmap/core/Link.h"
38 
39 #include <opencv2/core/core.hpp>
40 #include <list>
41 #include <stack>
42 #include <set>
43 
44 namespace rtabmap
45 {
46 
47 class EpipolarGeometry;
48 class Memory;
49 class BayesFilter;
50 class Signature;
51 class Optimizer;
52 class PythonInterface;
53 
54 class RTABMAP_CORE_EXPORT Rtabmap
55 {
56 public:
57  enum VhStrategy {kVhNone, kVhEpipolar, kVhUndef};
58 
59 public:
60  Rtabmap();
61  virtual ~Rtabmap();
62 
71  bool process(
72  const SensorData & data,
73  Transform odomPose,
74  const cv::Mat & odomCovariance = cv::Mat::eye(6,6,CV_64FC1),
75  const std::vector<float> & odomVelocity = std::vector<float>(),
76  const std::map<std::string, float> & externalStats = std::map<std::string, float>());
77  // for convenience
78  bool process(
79  const SensorData & data,
80  Transform odomPose,
81  float odomLinearVariance,
82  float odomAngularVariance,
83  const std::vector<float> & odomVelocity = std::vector<float>(),
84  const std::map<std::string, float> & externalStats = std::map<std::string, float>());
85  // for convenience, loop closure detection only
86  bool process(
87  const cv::Mat & image,
88  int id=0, const std::map<std::string, float> & externalStats = std::map<std::string, float>());
89 
102  void init(const ParametersMap & parameters, const std::string & databasePath = "", bool loadDatabaseParameters = false);
115  void init(const std::string & configFile = "", const std::string & databasePath = "", bool loadDatabaseParameters = false);
116 
124  void close(bool databaseSaved = true, const std::string & ouputDatabasePath = "");
125 
126  const std::string & getWorkingDir() const {return _wDir;}
127  bool isRGBDMode() const { return _rgbdSlamMode; }
128  int getLoopClosureId() const {return _loopClosureHypothesis.first;}
129  float getLoopClosureValue() const {return _loopClosureHypothesis.second;}
130  int getHighestHypothesisId() const {return _highestHypothesis.first;}
131  float getHighestHypothesisValue() const {return _highestHypothesis.second;}
132  int getLastLocationId() const;
133  std::list<int> getWM() const; // working memory
134  std::set<int> getSTM() const; // short-term memory
135  int getWMSize() const; // working memory size
136  int getSTMSize() const; // short-term memory size
137  std::map<int, int> getWeights() const;
138  int getTotalMemSize() const;
139  double getLastProcessTime() const {return _lastProcessTime;};
140  bool isInSTM(int locationId) const;
141  bool isIDsGenerated() const;
142  const Statistics & getStatistics() const;
143  const std::map<int, Transform> & getLocalOptimizedPoses() const {return _optimizedPoses;}
144  const std::multimap<int, Link> & getLocalConstraints() const {return _constraints;}
145  Transform getPose(int locationId) const;
146  Transform getMapCorrection() const {return _mapCorrection;}
147  const Memory * getMemory() const {return _memory;}
148  float getGoalReachedRadius() const {return _goalReachedRadius;}
149  float getLocalRadius() const {return _localRadius;}
150  const Transform & getLastLocalizationPose() const {return _lastLocalizationPose;}
151 
152  float getTimeThreshold() const {return _maxTimeAllowed;} // in ms
153  void setTimeThreshold(float maxTimeAllowed); // in ms
154  int getMemoryThreshold() const {return _maxMemoryAllowed;} // in nodes
155  void setMemoryThreshold(int maxMemoryAllowed); // in nodes
156 
157  void setInitialPose(const Transform & initialPose);
158  int triggerNewMap();
159  bool labelLocation(int id, const std::string & label);
167  bool setUserData(int id, const cv::Mat & data);
168  void generateDOTGraph(const std::string & path, int id=0, int margin=5);
169  void exportPoses(
170  const std::string & path,
171  bool optimized,
172  bool global,
173  int format // 0=raw, 1=rgbd-slam format, 2=KITTI format, 3=TORO, 4=g2o
174  );
175  void resetMemory();
176  void dumpPrediction() const;
177  void dumpData() const;
178  void parseParameters(const ParametersMap & parameters);
179  const ParametersMap & getParameters() const {return _parameters;}
180  void setWorkingDirectory(std::string path);
181  void rejectLastLoopClosure();
182  void deleteLastLocation();
183  void setOptimizedPoses(const std::map<int, Transform> & poses, const std::multimap<int, Link> & constraints);
184  Signature getSignatureCopy(int id, bool images, bool scan, bool userData, bool occupancyGrid, bool withWords, bool withGlobalDescriptors) const;
185  // Use getGraph() instead with withImages=true, withScan=true, withUserData=true and withGrid=true.
187  void get3DMap(std::map<int, Signature> & signatures,
188  std::map<int, Transform> & poses,
189  std::multimap<int, Link> & constraints,
190  bool optimized,
191  bool global) const;
192  void getGraph(std::map<int, Transform> & poses,
193  std::multimap<int, Link> & constraints,
194  bool optimized,
195  bool global,
196  std::map<int, Signature> * signatures = 0,
197  bool withImages = false,
198  bool withScan = false,
199  bool withUserData = false,
200  bool withGrid = false,
201  bool withWords = true,
202  bool withGlobalDescriptors = true) const;
203  std::map<int, Transform> getNodesInRadius(const Transform & pose, float radius, int k=0, std::map<int, float> * distsSqr=0); // If radius=0 and k=0, RGBD/LocalRadius is used. Can return landmarks.
204  std::map<int, Transform> getNodesInRadius(int nodeId, float radius, int k=0, std::map<int, float> * distsSqr=0); // If nodeId==0, return poses around latest node. If radius=0 and k=0, RGBD/LocalRadius is used. Can return landmarks and use landmark id (negative) as request.
205  int detectMoreLoopClosures(
206  float clusterRadiusMax = 0.5f,
207  float clusterAngle = M_PI/6.0f,
208  int iterations = 1,
209  bool intraSession = true,
210  bool interSession = true,
211  const ProgressState * state = 0,
212  float clusterRadiusMin = 0.0f);
213  bool globalBundleAdjustment(
214  int optimizerType = 1 /*g2o*/,
215  bool rematchFeatures = true,
216  int iterations = 0,
217  float pixelVariance = 0.0f);
218  int cleanupLocalGrids(
219  const std::map<int, Transform> & mapPoses,
220  const cv::Mat & map,
221  float xMin,
222  float yMin,
223  float cellSize,
224  int cropRadius = 1,
225  bool filterScans = false);
226  int refineLinks();
227  bool addLink(const Link & link);
228  cv::Mat getInformation(const cv::Mat & covariance) const;
229  void addNodesToRepublish(const std::vector<int> & ids);
230 
231  int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success
232  void clearPath(int status); // -1=failed 0=idle/executing 1=success
233  bool computePath(int targetNode, bool global);
234  bool computePath(const Transform & targetPose, float tolerance = -1.0f); // only in current optimized map, tolerance (m) < 0 means RGBD/LocalRadius, 0 means infinite
235  const std::vector<std::pair<int, Transform> > & getPath() const {return _path;}
236  std::vector<std::pair<int, Transform> > getPathNextPoses() const;
237  std::vector<int> getPathNextNodes() const;
238  int getPathCurrentGoalId() const;
239  unsigned int getPathCurrentIndex() const {return _pathCurrentIndex;}
240  unsigned int getPathCurrentGoalIndex() const {return _pathGoalIndex;}
241  const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;}
242 
243  std::map<int, Transform> getForwardWMPoses(int fromId, int maxNearestNeighbors, float radius, int maxDiffID) const;
244  std::map<int, std::map<int, Transform> > getPaths(const std::map<int, Transform> & poses, const Transform & target, int maxGraphDepth = 0) const;
245  void adjustLikelihood(std::map<int, float> & likelihood) const;
246  std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
247  const std::map<int, float> & likelihood) const;
248 
249 private:
250  void optimizeCurrentMap(int id,
251  bool lookInDatabase,
252  std::map<int, Transform> & optimizedPoses,
253  cv::Mat & covariance,
254  std::multimap<int, Link> * constraints = 0,
255  double * error = 0,
256  int * iterationsDone = 0) const;
257  std::map<int, Transform> optimizeGraph(
258  int fromId,
259  const std::set<int> & ids,
260  const std::map<int, Transform> & guessPoses,
261  bool lookInDatabase,
262  cv::Mat & covariance,
263  std::multimap<int, Link> * constraints = 0,
264  double * error = 0,
265  int * iterationsDone = 0) const;
266  void updateGoalIndex();
267  bool computePath(int targetNode, std::map<int, Transform> nodes, const std::multimap<int, rtabmap::Link> & constraints);
268 
269  void createGlobalScanMap();
270 
271  void setupLogFiles(bool overwrite = false);
272  void flushStatisticLogs();
273 
274 private:
275  // Modifiable parameters
283  float _maxTimeAllowed; // in ms
284  unsigned int _maxMemoryAllowed; // signatures count in WM
285  float _loopThr;
286  float _loopRatio;
291  unsigned int _maxRetrieved;
292  unsigned int _maxLocalRetrieved;
293  unsigned int _maxRepublished;
319  std::string _databasePath;
324  float _goalReachedRadius; // meters
332  bool _loopGPS;
339 
340  std::pair<int, float> _loopClosureHypothesis;
341  std::pair<int, float> _highestHypothesis;
347 
348  // Abstract classes containing all loop closure
349  // strategies for a type of signature or configuration.
354 
356 
357  FILE* _foutFloat;
358  FILE* _foutInt;
359  std::list<std::string> _bufferedLogsF;
360  std::list<std::string> _bufferedLogsI;
361 
363 
364  std::string _wDir;
365 
366  std::map<int, Transform> _optimizedPoses;
367  std::multimap<int, Link> _constraints;
369  Transform _mapCorrectionBackup; // used in localization mode when odom is lost
370  Transform _lastLocalizationPose; // Corrected odometry pose. In mapping mode, this corresponds to last pose return by getLocalOptimizedPoses().
371  int _lastLocalizationNodeId; // for localization mode
373  std::map<int, std::pair<cv::Point3d, Transform> > _gpsGeocentricCache;
376  std::map<int, Transform> _globalScanMapPoses;
377  std::map<int, Transform> _odomCachePoses; // used in localization mode to reject loop closures
378  std::multimap<int, Link> _odomCacheConstraints; // used in localization mode to reject loop closures
379  std::map<int, Transform> _markerPriors;
380 
381  std::set<int> _nodesToRepublish;
382 
383  // Planning stuff
385  std::vector<std::pair<int,Transform> > _path;
386  std::set<unsigned int> _pathUnreachableNodes;
387  unsigned int _pathCurrentIndex;
388  unsigned int _pathGoalIndex;
392 
393 #ifdef RTABMAP_PYTHON
394  PythonInterface * _python;
395 #endif
396 
397 };
398 
399 } // namespace rtabmap
400 #endif /* RTABMAP_H_ */
rtabmap::SensorData
Definition: SensorData.h:51
rtabmap::Rtabmap::_globalScanMap
LaserScan _globalScanMap
Definition: Rtabmap.h:375
rtabmap::Rtabmap::_distanceTravelled
float _distanceTravelled
Definition: Rtabmap.h:344
rtabmap::Rtabmap::_someNodesHaveBeenTransferred
bool _someNodesHaveBeenTransferred
Definition: Rtabmap.h:343
rtabmap::Rtabmap::_parameters
ParametersMap _parameters
Definition: Rtabmap.h:353
rtabmap::Statistics
Definition: Statistics.h:53
rtabmap::Rtabmap::_mapCorrectionBackup
Transform _mapCorrectionBackup
Definition: Rtabmap.h:369
rtabmap::Rtabmap::_bayesFilter
BayesFilter * _bayesFilter
Definition: Rtabmap.h:351
rtabmap::Rtabmap::_distanceTravelledSinceLastLocalization
float _distanceTravelledSinceLastLocalization
Definition: Rtabmap.h:345
rtabmap::Rtabmap::_foutFloat
FILE * _foutFloat
Definition: Rtabmap.h:357
rtabmap::Rtabmap::getMemory
const Memory * getMemory() const
Definition: Rtabmap.h:147
rtabmap::Rtabmap::getHighestHypothesisValue
float getHighestHypothesisValue() const
Definition: Rtabmap.h:131
rtabmap::Rtabmap::_gpsGeocentricCache
std::map< int, std::pair< cv::Point3d, Transform > > _gpsGeocentricCache
Definition: Rtabmap.h:373
rtabmap::Rtabmap::_pathStuckCount
int _pathStuckCount
Definition: Rtabmap.h:390
rtabmap::Rtabmap::_pathCurrentIndex
unsigned int _pathCurrentIndex
Definition: Rtabmap.h:387
rtabmap::Rtabmap::_startNewMapOnLoopClosure
bool _startNewMapOnLoopClosure
Definition: Rtabmap.h:322
rtabmap::Rtabmap::getLocalOptimizedPoses
const std::map< int, Transform > & getLocalOptimizedPoses() const
Definition: Rtabmap.h:143
rtabmap::Rtabmap::_statisticLogsBufferedInRAM
bool _statisticLogsBufferedInRAM
Definition: Rtabmap.h:295
rtabmap::Optimizer
Definition: Optimizer.h:61
rtabmap::RTABMAP_DEPRECATED
RTABMAP_DEPRECATED(typedef SensorData Image, "rtabmap::Image class is renamed to rtabmap::SensorData, use the last one instead.")
SensorData.h
rtabmap::Rtabmap::_odomCachePoses
std::map< int, Transform > _odomCachePoses
Definition: Rtabmap.h:377
rtabmap::Rtabmap::_loopClosureHypothesis
std::pair< int, float > _loopClosureHypothesis
Definition: Rtabmap.h:340
rtabmap::Rtabmap::_pathStatus
int _pathStatus
Definition: Rtabmap.h:384
rtabmap::Rtabmap::_verifyLoopClosureHypothesis
bool _verifyLoopClosureHypothesis
Definition: Rtabmap.h:290
rtabmap::Rtabmap::getGoalReachedRadius
float getGoalReachedRadius() const
Definition: Rtabmap.h:148
rtabmap::Rtabmap::_bufferedLogsI
std::list< std::string > _bufferedLogsI
Definition: Rtabmap.h:360
rtabmap::Rtabmap::_maxOdomCacheSize
int _maxOdomCacheSize
Definition: Rtabmap.h:333
rtabmap::Rtabmap::_proximityFilteringRadius
float _proximityFilteringRadius
Definition: Rtabmap.h:314
rtabmap::Rtabmap::_localizationPriorInf
double _localizationPriorInf
Definition: Rtabmap.h:335
rtabmap::Rtabmap::getPathCurrentIndex
unsigned int getPathCurrentIndex() const
Definition: Rtabmap.h:239
rtabmap::Rtabmap::_odomCacheConstraints
std::multimap< int, Link > _odomCacheConstraints
Definition: Rtabmap.h:378
state
RecoveryProgressState state
Definition: tools/Recovery/main.cpp:56
rtabmap::Rtabmap::_lastProcessTime
double _lastProcessTime
Definition: Rtabmap.h:342
rtabmap::Rtabmap::_optimizationMaxError
float _optimizationMaxError
Definition: Rtabmap.h:321
ProgressState.h
rtabmap::Rtabmap::_maxRepublished
unsigned int _maxRepublished
Definition: Rtabmap.h:293
rtabmap::Rtabmap::_maxTimeAllowed
float _maxTimeAllowed
Definition: Rtabmap.h:283
rtabmap::Rtabmap::getLastLocalizationPose
const Transform & getLastLocalizationPose() const
Definition: Rtabmap.h:150
rtabmap::Rtabmap::getLoopClosureId
int getLoopClosureId() const
Definition: Rtabmap.h:128
rtabmap::Rtabmap::_proximityRawPosesUsed
bool _proximityRawPosesUsed
Definition: Rtabmap.h:315
rtabmap::Rtabmap::_proximityMergedScanCovFactor
double _proximityMergedScanCovFactor
Definition: Rtabmap.h:318
rtabmap::LaserScan
Definition: LaserScan.h:37
rtabmap::Rtabmap::getLoopClosureValue
float getLoopClosureValue() const
Definition: Rtabmap.h:129
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::Rtabmap::_globalScanMapPoses
std::map< int, Transform > _globalScanMapPoses
Definition: Rtabmap.h:376
rtabmap::Rtabmap::_proximityOdomGuess
bool _proximityOdomGuess
Definition: Rtabmap.h:317
rtabmap::Rtabmap::getPathTransformToGoal
const Transform & getPathTransformToGoal() const
Definition: Rtabmap.h:241
rtabmap::Rtabmap::_neighborLinkRefining
bool _neighborLinkRefining
Definition: Rtabmap.h:304
rtabmap::Rtabmap::_publishLikelihood
bool _publishLikelihood
Definition: Rtabmap.h:279
rtabmap::Rtabmap::_forceOdom3doF
bool _forceOdom3doF
Definition: Rtabmap.h:329
rtabmap::Rtabmap::_restartAtOrigin
bool _restartAtOrigin
Definition: Rtabmap.h:330
rtabmap::Rtabmap::_proximityAngle
float _proximityAngle
Definition: Rtabmap.h:316
rtabmap::Rtabmap::_wDir
std::string _wDir
Definition: Rtabmap.h:364
Parameters.h
rtabmap::Rtabmap::getLastProcessTime
double getLastProcessTime() const
Definition: Rtabmap.h:139
rtabmap::Rtabmap::_pathStuckIterations
int _pathStuckIterations
Definition: Rtabmap.h:326
rtabmap::Rtabmap::_pathUnreachableNodes
std::set< unsigned int > _pathUnreachableNodes
Definition: Rtabmap.h:386
rtabmap::Rtabmap::_foutInt
FILE * _foutInt
Definition: Rtabmap.h:358
rtabmap::Rtabmap::_constraints
std::multimap< int, Link > _constraints
Definition: Rtabmap.h:367
rtabmap::Rtabmap::_highestHypothesis
std::pair< int, float > _highestHypothesis
Definition: Rtabmap.h:341
rtabmap::Rtabmap::_localImmunizationRatio
float _localImmunizationRatio
Definition: Rtabmap.h:310
rtabmap::Rtabmap::_lastLocalizationNodeId
int _lastLocalizationNodeId
Definition: Rtabmap.h:371
rtabmap::Rtabmap::_epipolarGeometry
EpipolarGeometry * _epipolarGeometry
Definition: Rtabmap.h:350
rtabmap::Rtabmap::_goalsSavedInUserData
bool _goalsSavedInUserData
Definition: Rtabmap.h:325
rtabmap::Rtabmap::getLocalRadius
float getLocalRadius() const
Definition: Rtabmap.h:149
rtabmap::Rtabmap::_startNewMapOnGoodSignature
bool _startNewMapOnGoodSignature
Definition: Rtabmap.h:323
rtabmap::Rtabmap::getWorkingDir
const std::string & getWorkingDir() const
Definition: Rtabmap.h:126
rtabmap::Rtabmap::_rawDataKept
bool _rawDataKept
Definition: Rtabmap.h:294
rtabmap::Rtabmap::getParameters
const ParametersMap & getParameters() const
Definition: Rtabmap.h:179
rtabmap::Rtabmap::_loopClosureIdentityGuess
bool _loopClosureIdentityGuess
Definition: Rtabmap.h:308
rtabmap::Rtabmap::_localizationCovariance
cv::Mat _localizationCovariance
Definition: Rtabmap.h:372
rtabmap::Rtabmap::_maxMemoryAllowed
unsigned int _maxMemoryAllowed
Definition: Rtabmap.h:284
rtabmap::Rtabmap::_rgbdSlamMode
bool _rgbdSlamMode
Definition: Rtabmap.h:298
rtabmap::Rtabmap::_publishLastSignatureData
bool _publishLastSignatureData
Definition: Rtabmap.h:277
rtabmap::Rtabmap::getTimeThreshold
float getTimeThreshold() const
Definition: Rtabmap.h:152
rtabmap::Rtabmap::_pathStuckDistance
float _pathStuckDistance
Definition: Rtabmap.h:391
rtabmap::Rtabmap::_optimizedPoses
std::map< int, Transform > _optimizedPoses
Definition: Rtabmap.h:366
rtabmap::Rtabmap::_maxLoopClosureDistance
float _maxLoopClosureDistance
Definition: Rtabmap.h:289
rtabmap::Rtabmap::_mapCorrection
Transform _mapCorrection
Definition: Rtabmap.h:368
rtabmap::Rtabmap::_computeRMSE
bool _computeRMSE
Definition: Rtabmap.h:281
rtabmap::Rtabmap::_maxRetrieved
unsigned int _maxRetrieved
Definition: Rtabmap.h:291
rtabmap::Rtabmap::_pathLinearVelocity
float _pathLinearVelocity
Definition: Rtabmap.h:327
rtabmap::PythonInterface
Definition: PythonInterface.h:26
rtabmap::Rtabmap::_graphOptimizer
Optimizer * _graphOptimizer
Definition: Rtabmap.h:352
rtabmap::Rtabmap::isRGBDMode
bool isRGBDMode() const
Definition: Rtabmap.h:127
rtabmap::Rtabmap::_proximityMaxPaths
int _proximityMaxPaths
Definition: Rtabmap.h:312
exportPoses
bool exportPoses
Definition: tools/Reprocess/main.cpp:131
rtabmap::Rtabmap::_goalReachedRadius
float _goalReachedRadius
Definition: Rtabmap.h:324
rtabmap::Rtabmap::_createGlobalScanMap
bool _createGlobalScanMap
Definition: Rtabmap.h:336
rtabmap::Rtabmap::_lastLocalizationPose
Transform _lastLocalizationPose
Definition: Rtabmap.h:370
rtabmap::Rtabmap::_virtualPlaceLikelihoodRatio
int _virtualPlaceLikelihoodRatio
Definition: Rtabmap.h:288
rtabmap::graph::getPaths
std::list< std::map< int, Transform > > RTABMAP_CORE_EXPORT getPaths(std::map< int, Transform > poses, const std::multimap< int, Link > &links)
Definition: Graph.cpp:2395
rtabmap::ProgressState
Definition: ProgressState.h:35
rtabmap::Rtabmap::_pathGoalIndex
unsigned int _pathGoalIndex
Definition: Rtabmap.h:388
rtabmap::Rtabmap::_nodesToRepublish
std::set< int > _nodesToRepublish
Definition: Rtabmap.h:381
rtabmap::Rtabmap::_publishStats
bool _publishStats
Definition: Rtabmap.h:276
rtabmap::Rtabmap::_rgbdAngularSpeedUpdate
float _rgbdAngularSpeedUpdate
Definition: Rtabmap.h:302
rtabmap::Rtabmap::_memory
Memory * _memory
Definition: Rtabmap.h:355
rtabmap::Rtabmap::_currentSessionHasGPS
bool _currentSessionHasGPS
Definition: Rtabmap.h:374
rtabmap::Rtabmap::_localizationSmoothing
bool _localizationSmoothing
Definition: Rtabmap.h:334
rtabmap::Rtabmap::getPathCurrentGoalIndex
unsigned int getPathCurrentGoalIndex() const
Definition: Rtabmap.h:240
rtabmap::Rtabmap::_rgbdLinearUpdate
float _rgbdLinearUpdate
Definition: Rtabmap.h:299
rtabmap::Rtabmap::_markerPriors
std::map< int, Transform > _markerPriors
Definition: Rtabmap.h:379
M_PI
#define M_PI
Definition: tango-gl/include/tango-gl/util.h:66
rtabmap_netvlad.init
def init(descriptorDim)
Definition: rtabmap_netvlad.py:30
rtabmap::Rtabmap::_bufferedLogsF
std::list< std::string > _bufferedLogsF
Definition: Rtabmap.h:359
rtabmap::Rtabmap::getPathStatus
int getPathStatus() const
Definition: Rtabmap.h:231
getPose
Pose3_ getPose(const Expression< PinholeCamera< CALIBRATION > > &cam)
rtabmap::Rtabmap::_pathAngularVelocity
float _pathAngularVelocity
Definition: Rtabmap.h:328
rtabmap::Rtabmap::_maxLocalRetrieved
unsigned int _maxLocalRetrieved
Definition: Rtabmap.h:292
rtabmap::Rtabmap::getPath
const std::vector< std::pair< int, Transform > > & getPath() const
Definition: Rtabmap.h:235
rtabmap::Transform
Definition: Transform.h:41
rtabmap::Rtabmap::_optimizeFromGraphEnd
bool _optimizeFromGraphEnd
Definition: Rtabmap.h:320
rtabmap::Rtabmap::_pathTransformToGoal
Transform _pathTransformToGoal
Definition: Rtabmap.h:389
rtabmap::Rtabmap::_proximityByTime
bool _proximityByTime
Definition: Rtabmap.h:305
rtabmap::Rtabmap::_rgbdLinearSpeedUpdate
float _rgbdLinearSpeedUpdate
Definition: Rtabmap.h:301
rtabmap::Rtabmap::_statisticLoggedHeaders
bool _statisticLoggedHeaders
Definition: Rtabmap.h:297
rtabmap::Rtabmap::statistics_
Statistics statistics_
Definition: Rtabmap.h:362
rtabmap::Rtabmap::_path
std::vector< std::pair< int, Transform > > _path
Definition: Rtabmap.h:385
rtabmap::Rtabmap::_localRadius
float _localRadius
Definition: Rtabmap.h:309
rtabmap::Rtabmap::_markerPriorsAngularVariance
float _markerPriorsAngularVariance
Definition: Rtabmap.h:338
rtabmap::Rtabmap::_proximityMaxNeighbors
int _proximityMaxNeighbors
Definition: Rtabmap.h:313
rtabmap::Rtabmap::_loopThr
float _loopThr
Definition: Rtabmap.h:285
rtabmap::Rtabmap::getHighestHypothesisId
int getHighestHypothesisId() const
Definition: Rtabmap.h:130
rtabmap::Rtabmap::_loopGPS
bool _loopGPS
Definition: Rtabmap.h:332
rtabmap::Rtabmap::_newMapOdomChangeDistance
float _newMapOdomChangeDistance
Definition: Rtabmap.h:303
rtabmap::Rtabmap::_optimizeFromGraphEndChanged
bool _optimizeFromGraphEndChanged
Definition: Rtabmap.h:346
rtabmap::Rtabmap::_saveWMState
bool _saveWMState
Definition: Rtabmap.h:282
Statistics.h
rtabmap::Rtabmap::_rgbdAngularUpdate
float _rgbdAngularUpdate
Definition: Rtabmap.h:300
rtabmap::Rtabmap::_databasePath
std::string _databasePath
Definition: Rtabmap.h:319
rtabmap::Memory
Definition: Memory.h:64
rtabmap::Rtabmap::getMapCorrection
Transform getMapCorrection() const
Definition: Rtabmap.h:146
rtabmap::Rtabmap
Definition: Rtabmap.h:54
rtabmap::Rtabmap::_proximityMaxGraphDepth
int _proximityMaxGraphDepth
Definition: Rtabmap.h:311
rtabmap::Rtabmap::getLocalConstraints
const std::multimap< int, Link > & getLocalConstraints() const
Definition: Rtabmap.h:144
rtabmap::Rtabmap::_publishRAMUsage
bool _publishRAMUsage
Definition: Rtabmap.h:280
rtabmap::EpipolarGeometry
Definition: EpipolarGeometry.h:45
rtabmap::Rtabmap::_markerPriorsLinearVariance
float _markerPriorsLinearVariance
Definition: Rtabmap.h:337
rtabmap::Rtabmap::VhStrategy
VhStrategy
Definition: Rtabmap.h:57
rtabmap::Rtabmap::_proximityBySpace
bool _proximityBySpace
Definition: Rtabmap.h:306
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::Rtabmap::_publishPdf
bool _publishPdf
Definition: Rtabmap.h:278
rtabmap::Rtabmap::_scanMatchingIdsSavedInLinks
bool _scanMatchingIdsSavedInLinks
Definition: Rtabmap.h:307
rtabmap::graph::computePath
std::list< std::pair< int, Transform > > RTABMAP_CORE_EXPORT computePath(const std::map< int, rtabmap::Transform > &poses, const std::multimap< int, int > &links, int from, int to, bool updateNewCosts=false)
Definition: Graph.cpp:1756
rtabmap::Rtabmap::_aggressiveLoopThr
float _aggressiveLoopThr
Definition: Rtabmap.h:287
rtabmap::BayesFilter
Definition: BayesFilter.h:44
rtabmap::Rtabmap::getMemoryThreshold
int getMemoryThreshold() const
Definition: Rtabmap.h:154
rtabmap::graph::getNodesInRadius
RTABMAP_DEPRECATED std::map< int, float > RTABMAP_CORE_EXPORT getNodesInRadius(int nodeId, const std::map< int, Transform > &nodes, float radius)
Definition: Graph.cpp:2328
rtabmap::Rtabmap::_statisticLogged
bool _statisticLogged
Definition: Rtabmap.h:296
rtabmap::Rtabmap::_loopCovLimited
bool _loopCovLimited
Definition: Rtabmap.h:331
rtabmap::Signature
Definition: Signature.h:48
rtabmap::Rtabmap::_loopRatio
float _loopRatio
Definition: Rtabmap.h:286


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:15