#include <Rtabmap.h>
Public Types | |
enum | VhStrategy { kVhNone, kVhEpipolar, kVhUndef } |
Public Member Functions | |
void | adjustLikelihood (std::map< int, float > &likelihood) const |
void | clearPath () |
void | close () |
bool | computePath (int targetNode, bool global) |
bool | computePath (const Transform &targetPose, bool global) |
void | dumpData () const |
void | dumpPrediction () const |
void | generateDOTGraph (const std::string &path, int id=0, int margin=5) |
void | generateTOROGraph (const std::string &path, bool optimized, bool global) |
void | get3DMap (std::map< int, Signature > &signatures, std::map< int, Transform > &poses, std::multimap< int, Link > &constraints, std::map< int, int > &mapIds, std::map< int, double > &stamps, std::map< int, std::string > &labels, std::map< int, std::vector< unsigned char > > &userDatas, bool optimized, bool global) const |
std::map< int, Transform > | getForwardWMPoses (int fromId, int maxNearestNeighbors, float radius, int maxDiffID) const |
float | getGoalReachedRadius () const |
void | getGraph (std::map< int, Transform > &poses, std::multimap< int, Link > &constraints, std::map< int, int > &mapIds, std::map< int, double > &stamps, std::map< int, std::string > &labels, std::map< int, std::vector< unsigned char > > &userDatas, bool optimized, bool global) |
int | getHighestHypothesisId () const |
float | getHighestHypothesisValue () const |
int | getLastLocationId () const |
double | getLastProcessTime () const |
const std::map< int, Transform > & | getLocalOptimizedPoses () const |
float | getLocalRadius () const |
int | getLoopClosureId () const |
float | getLoopClosureValue () const |
Transform | getMapCorrection () const |
const Memory * | getMemory () const |
const std::vector< std::pair < int, Transform > > & | getPath () const |
int | getPathCurrentGoalId () const |
std::vector< int > | getPathNextNodes () const |
std::vector< std::pair< int, Transform > > | getPathNextPoses () const |
std::list< std::map< int, Transform > > | getPaths (std::map< int, Transform > poses) const |
const Transform & | getPathTransformToGoal () const |
Transform | getPose (int locationId) const |
const Statistics & | getStatistics () const |
std::set< int > | getSTM () const |
int | getSTMSize () const |
float | getTimeThreshold () const |
int | getTotalMemSize () const |
std::map< int, int > | getWeights () const |
std::list< int > | getWM () const |
int | getWMSize () const |
std::multimap< int, cv::KeyPoint > | getWords (int locationId) const |
const std::string & | getWorkingDir () const |
void | init (const ParametersMap ¶meters, const std::string &databasePath="") |
void | init (const std::string &configFile="", const std::string &databasePath="") |
bool | isIDsGenerated () const |
bool | isInSTM (int locationId) const |
bool | labelLocation (int id, const std::string &label) |
void | parseParameters (const ParametersMap ¶meters) |
bool | process (const cv::Mat &image, int id=0) |
bool | process (const SensorData &data) |
void | rejectLoopClosure (int oldId, int newId) |
void | resetMemory () |
Rtabmap () | |
std::pair< int, float > | selectHypothesis (const std::map< int, float > &posterior, const std::map< int, float > &likelihood) const |
void | setTimeThreshold (float maxTimeAllowed) |
bool | setUserData (int id, const std::vector< unsigned char > &data) |
void | setWorkingDirectory (std::string path) |
int | triggerNewMap () |
virtual | ~Rtabmap () |
Static Public Member Functions | |
static std::string | getVersion () |
static void | readParameters (const std::string &configFile, ParametersMap ¶meters) |
static void | writeParameters (const std::string &configFile, const ParametersMap ¶meters) |
Private Member Functions | |
bool | computePath (int targetNode, std::map< int, Transform > nodes, const std::multimap< int, rtabmap::Link > &constraints) |
void | flushStatisticLogs () |
void | optimizeCurrentMap (int id, bool lookInDatabase, std::map< int, Transform > &optimizedPoses, std::multimap< int, Link > *constraints=0) const |
std::map< int, Transform > | optimizeGraph (int fromId, const std::set< int > &ids, bool lookInDatabase, std::multimap< int, Link > *constraints=0) const |
void | setupLogFiles (bool overwrite=false) |
void | updateGoalIndex () |
Private Attributes | |
BayesFilter * | _bayesFilter |
std::list< std::string > | _bufferedLogsF |
std::list< std::string > | _bufferedLogsI |
std::multimap< int, Link > | _constraints |
std::string | _databasePath |
EpipolarGeometry * | _epipolarGeometry |
FILE * | _foutFloat |
FILE * | _foutInt |
int | _globalLoopClosureIcpType |
float | _goalReachedRadius |
bool | _goalsSavedInUserData |
graph::Optimizer * | _graphOptimizer |
std::pair< int, float > | _highestHypothesis |
Transform | _lastLocalizationPose |
double | _lastProcessTime |
int | _localDetectMaxGraphDepth |
float | _localImmunizationRatio |
bool | _localLoopClosureDetectionSpace |
bool | _localLoopClosureDetectionTime |
float | _localPathFilteringRadius |
bool | _localPathOdomPosesUsed |
float | _localRadius |
std::pair< int, float > | _loopClosureHypothesis |
float | _loopRatio |
float | _loopThr |
Transform | _mapCorrection |
Transform | _mapTransform |
unsigned int | _maxLocalRetrieved |
unsigned int | _maxMemoryAllowed |
unsigned int | _maxRetrieved |
float | _maxTimeAllowed |
Memory * | _memory |
ParametersMap | _modifiedParameters |
float | _newMapOdomChangeDistance |
std::map< int, Transform > | _optimizedPoses |
bool | _optimizeFromGraphEnd |
std::vector< std::pair< int, Transform > > | _path |
unsigned int | _pathCurrentIndex |
unsigned int | _pathGoalIndex |
Transform | _pathTransformToGoal |
bool | _planVirtualLinks |
bool | _poseScanMatching |
bool | _publishLastSignature |
bool | _publishLikelihood |
bool | _publishPdf |
bool | _publishStats |
int | _reextractFeatureType |
bool | _reextractLoopClosureFeatures |
int | _reextractMaxWords |
float | _reextractNNDR |
int | _reextractNNType |
float | _rgbdAngularUpdate |
float | _rgbdLinearUpdate |
bool | _rgbdSlamMode |
bool | _startNewMapOnLoopClosure |
bool | _statisticLogged |
bool | _statisticLoggedHeaders |
bool | _statisticLogsBufferedInRAM |
std::string | _wDir |
Statistics | statistics_ |
Definition at line 76 of file Rtabmap.cpp.
rtabmap::Rtabmap::~Rtabmap | ( | ) | [virtual] |
Definition at line 132 of file Rtabmap.cpp.
void rtabmap::Rtabmap::adjustLikelihood | ( | std::map< int, float > & | likelihood | ) | const |
Definition at line 2620 of file Rtabmap.cpp.
void rtabmap::Rtabmap::clearPath | ( | ) |
Definition at line 2859 of file Rtabmap.cpp.
void rtabmap::Rtabmap::close | ( | ) |
Definition at line 309 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::computePath | ( | int | targetNode, |
bool | global | ||
) |
Definition at line 2983 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::computePath | ( | const Transform & | targetPose, |
bool | global | ||
) |
Definition at line 3012 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::computePath | ( | int | targetNode, |
std::map< int, Transform > | nodes, | ||
const std::multimap< int, rtabmap::Link > & | constraints | ||
) | [private] |
Definition at line 2871 of file Rtabmap.cpp.
void rtabmap::Rtabmap::dumpData | ( | ) | const |
Definition at line 2399 of file Rtabmap.cpp.
void rtabmap::Rtabmap::dumpPrediction | ( | ) | const |
Definition at line 2691 of file Rtabmap.cpp.
void rtabmap::Rtabmap::flushStatisticLogs | ( | ) | [private] |
Definition at line 235 of file Rtabmap.cpp.
void rtabmap::Rtabmap::generateDOTGraph | ( | const std::string & | path, |
int | id = 0 , |
||
int | margin = 5 |
||
) |
Definition at line 681 of file Rtabmap.cpp.
void rtabmap::Rtabmap::generateTOROGraph | ( | const std::string & | path, |
bool | optimized, | ||
bool | global | ||
) |
Definition at line 713 of file Rtabmap.cpp.
void rtabmap::Rtabmap::get3DMap | ( | std::map< int, Signature > & | signatures, |
std::map< int, Transform > & | poses, | ||
std::multimap< int, Link > & | constraints, | ||
std::map< int, int > & | mapIds, | ||
std::map< int, double > & | stamps, | ||
std::map< int, std::string > & | labels, | ||
std::map< int, std::vector< unsigned char > > & | userDatas, | ||
bool | optimized, | ||
bool | global | ||
) | const |
Definition at line 2724 of file Rtabmap.cpp.
std::map< int, Transform > rtabmap::Rtabmap::getForwardWMPoses | ( | int | fromId, |
int | maxNearestNeighbors, | ||
float | radius, | ||
int | maxDiffID | ||
) | const |
Definition at line 2410 of file Rtabmap.cpp.
float rtabmap::Rtabmap::getGoalReachedRadius | ( | ) | const [inline] |
void rtabmap::Rtabmap::getGraph | ( | std::map< int, Transform > & | poses, |
std::multimap< int, Link > & | constraints, | ||
std::map< int, int > & | mapIds, | ||
std::map< int, double > & | stamps, | ||
std::map< int, std::string > & | labels, | ||
std::map< int, std::vector< unsigned char > > & | userDatas, | ||
bool | optimized, | ||
bool | global | ||
) |
Definition at line 2803 of file Rtabmap.cpp.
int rtabmap::Rtabmap::getHighestHypothesisId | ( | ) | const [inline] |
float rtabmap::Rtabmap::getHighestHypothesisValue | ( | ) | const [inline] |
int rtabmap::Rtabmap::getLastLocationId | ( | ) | const |
Definition at line 489 of file Rtabmap.cpp.
double rtabmap::Rtabmap::getLastProcessTime | ( | ) | const [inline] |
const std::map<int, Transform>& rtabmap::Rtabmap::getLocalOptimizedPoses | ( | ) | const [inline] |
float rtabmap::Rtabmap::getLocalRadius | ( | ) | const [inline] |
int rtabmap::Rtabmap::getLoopClosureId | ( | ) | const [inline] |
float rtabmap::Rtabmap::getLoopClosureValue | ( | ) | const [inline] |
Transform rtabmap::Rtabmap::getMapCorrection | ( | ) | const [inline] |
const Memory* rtabmap::Rtabmap::getMemory | ( | ) | const [inline] |
const std::vector<std::pair<int, Transform> >& rtabmap::Rtabmap::getPath | ( | ) | const [inline] |
int rtabmap::Rtabmap::getPathCurrentGoalId | ( | ) | const |
Definition at line 3114 of file Rtabmap.cpp.
std::vector< int > rtabmap::Rtabmap::getPathNextNodes | ( | ) | const |
Definition at line 3089 of file Rtabmap.cpp.
std::vector< std::pair< int, Transform > > rtabmap::Rtabmap::getPathNextPoses | ( | ) | const |
Definition at line 3064 of file Rtabmap.cpp.
std::list< std::map< int, Transform > > rtabmap::Rtabmap::getPaths | ( | std::map< int, Transform > | poses | ) | const |
Definition at line 2527 of file Rtabmap.cpp.
const Transform& rtabmap::Rtabmap::getPathTransformToGoal | ( | ) | const [inline] |
Transform rtabmap::Rtabmap::getPose | ( | int | locationId | ) | const |
Definition at line 615 of file Rtabmap.cpp.
const Statistics & rtabmap::Rtabmap::getStatistics | ( | ) | const |
Definition at line 592 of file Rtabmap.cpp.
std::set< int > rtabmap::Rtabmap::getSTM | ( | ) | const |
Definition at line 530 of file Rtabmap.cpp.
int rtabmap::Rtabmap::getSTMSize | ( | ) | const |
Definition at line 539 of file Rtabmap.cpp.
float rtabmap::Rtabmap::getTimeThreshold | ( | ) | const [inline] |
int rtabmap::Rtabmap::getTotalMemSize | ( | ) | const |
Definition at line 548 of file Rtabmap.cpp.
std::string rtabmap::Rtabmap::getVersion | ( | ) | [static] |
Definition at line 137 of file Rtabmap.cpp.
std::map< int, int > rtabmap::Rtabmap::getWeights | ( | ) | const |
Definition at line 519 of file Rtabmap.cpp.
std::list< int > rtabmap::Rtabmap::getWM | ( | ) | const |
Definition at line 499 of file Rtabmap.cpp.
int rtabmap::Rtabmap::getWMSize | ( | ) | const |
Definition at line 510 of file Rtabmap.cpp.
std::multimap< int, cv::KeyPoint > rtabmap::Rtabmap::getWords | ( | int | locationId | ) | const |
Definition at line 561 of file Rtabmap.cpp.
const std::string& rtabmap::Rtabmap::getWorkingDir | ( | ) | const [inline] |
void rtabmap::Rtabmap::init | ( | const ParametersMap & | parameters, |
const std::string & | databasePath = "" |
||
) |
Definition at line 257 of file Rtabmap.cpp.
void rtabmap::Rtabmap::init | ( | const std::string & | configFile = "" , |
const std::string & | databasePath = "" |
||
) |
Definition at line 295 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::isIDsGenerated | ( | ) | const |
Definition at line 583 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::isInSTM | ( | int | locationId | ) | const |
Definition at line 574 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::labelLocation | ( | int | id, |
const std::string & | label | ||
) |
Definition at line 641 of file Rtabmap.cpp.
void rtabmap::Rtabmap::optimizeCurrentMap | ( | int | id, |
bool | lookInDatabase, | ||
std::map< int, Transform > & | optimizedPoses, | ||
std::multimap< int, Link > * | constraints = 0 |
||
) | const [private] |
Definition at line 2556 of file Rtabmap.cpp.
std::map< int, Transform > rtabmap::Rtabmap::optimizeGraph | ( | int | fromId, |
const std::set< int > & | ids, | ||
bool | lookInDatabase, | ||
std::multimap< int, Link > * | constraints = 0 |
||
) | const [private] |
Definition at line 2587 of file Rtabmap.cpp.
void rtabmap::Rtabmap::parseParameters | ( | const ParametersMap & | parameters | ) |
Definition at line 359 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::process | ( | const cv::Mat & | image, |
int | id = 0 |
||
) |
Definition at line 2336 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::process | ( | const SensorData & | data | ) |
Definition at line 768 of file Rtabmap.cpp.
void rtabmap::Rtabmap::readParameters | ( | const std::string & | configFile, |
ParametersMap & | parameters | ||
) | [static] |
Definition at line 3271 of file Rtabmap.cpp.
void rtabmap::Rtabmap::rejectLoopClosure | ( | int | oldId, |
int | newId | ||
) |
Definition at line 2381 of file Rtabmap.cpp.
void rtabmap::Rtabmap::resetMemory | ( | ) |
Definition at line 734 of file Rtabmap.cpp.
std::pair<int, float> rtabmap::Rtabmap::selectHypothesis | ( | const std::map< int, float > & | posterior, |
const std::map< int, float > & | likelihood | ||
) | const |
void rtabmap::Rtabmap::setTimeThreshold | ( | float | maxTimeAllowed | ) |
Definition at line 2342 of file Rtabmap.cpp.
void rtabmap::Rtabmap::setupLogFiles | ( | bool | overwrite = false | ) | [private] |
Definition at line 143 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::setUserData | ( | int | id, |
const std::vector< unsigned char > & | data | ||
) |
Definition at line 661 of file Rtabmap.cpp.
void rtabmap::Rtabmap::setWorkingDirectory | ( | std::string | path | ) |
Definition at line 2357 of file Rtabmap.cpp.
int rtabmap::Rtabmap::triggerNewMap | ( | ) |
Definition at line 628 of file Rtabmap.cpp.
void rtabmap::Rtabmap::updateGoalIndex | ( | ) | [private] |
Definition at line 3124 of file Rtabmap.cpp.
void rtabmap::Rtabmap::writeParameters | ( | const std::string & | configFile, |
const ParametersMap & | parameters | ||
) | [static] |
Definition at line 3330 of file Rtabmap.cpp.
BayesFilter* rtabmap::Rtabmap::_bayesFilter [private] |
std::list<std::string> rtabmap::Rtabmap::_bufferedLogsF [private] |
std::list<std::string> rtabmap::Rtabmap::_bufferedLogsI [private] |
std::multimap<int, Link> rtabmap::Rtabmap::_constraints [private] |
std::string rtabmap::Rtabmap::_databasePath [private] |
FILE* rtabmap::Rtabmap::_foutFloat [private] |
FILE* rtabmap::Rtabmap::_foutInt [private] |
int rtabmap::Rtabmap::_globalLoopClosureIcpType [private] |
float rtabmap::Rtabmap::_goalReachedRadius [private] |
bool rtabmap::Rtabmap::_goalsSavedInUserData [private] |
std::pair<int, float> rtabmap::Rtabmap::_highestHypothesis [private] |
double rtabmap::Rtabmap::_lastProcessTime [private] |
int rtabmap::Rtabmap::_localDetectMaxGraphDepth [private] |
float rtabmap::Rtabmap::_localImmunizationRatio [private] |
bool rtabmap::Rtabmap::_localLoopClosureDetectionSpace [private] |
bool rtabmap::Rtabmap::_localLoopClosureDetectionTime [private] |
float rtabmap::Rtabmap::_localPathFilteringRadius [private] |
bool rtabmap::Rtabmap::_localPathOdomPosesUsed [private] |
float rtabmap::Rtabmap::_localRadius [private] |
std::pair<int, float> rtabmap::Rtabmap::_loopClosureHypothesis [private] |
float rtabmap::Rtabmap::_loopRatio [private] |
float rtabmap::Rtabmap::_loopThr [private] |
Transform rtabmap::Rtabmap::_mapCorrection [private] |
Transform rtabmap::Rtabmap::_mapTransform [private] |
unsigned int rtabmap::Rtabmap::_maxLocalRetrieved [private] |
unsigned int rtabmap::Rtabmap::_maxMemoryAllowed [private] |
unsigned int rtabmap::Rtabmap::_maxRetrieved [private] |
float rtabmap::Rtabmap::_maxTimeAllowed [private] |
Memory* rtabmap::Rtabmap::_memory [private] |
float rtabmap::Rtabmap::_newMapOdomChangeDistance [private] |
std::map<int, Transform> rtabmap::Rtabmap::_optimizedPoses [private] |
bool rtabmap::Rtabmap::_optimizeFromGraphEnd [private] |
std::vector<std::pair<int,Transform> > rtabmap::Rtabmap::_path [private] |
unsigned int rtabmap::Rtabmap::_pathCurrentIndex [private] |
unsigned int rtabmap::Rtabmap::_pathGoalIndex [private] |
bool rtabmap::Rtabmap::_planVirtualLinks [private] |
bool rtabmap::Rtabmap::_poseScanMatching [private] |
bool rtabmap::Rtabmap::_publishLastSignature [private] |
bool rtabmap::Rtabmap::_publishLikelihood [private] |
bool rtabmap::Rtabmap::_publishPdf [private] |
bool rtabmap::Rtabmap::_publishStats [private] |
int rtabmap::Rtabmap::_reextractFeatureType [private] |
bool rtabmap::Rtabmap::_reextractLoopClosureFeatures [private] |
int rtabmap::Rtabmap::_reextractMaxWords [private] |
float rtabmap::Rtabmap::_reextractNNDR [private] |
int rtabmap::Rtabmap::_reextractNNType [private] |
float rtabmap::Rtabmap::_rgbdAngularUpdate [private] |
float rtabmap::Rtabmap::_rgbdLinearUpdate [private] |
bool rtabmap::Rtabmap::_rgbdSlamMode [private] |
bool rtabmap::Rtabmap::_startNewMapOnLoopClosure [private] |
bool rtabmap::Rtabmap::_statisticLogged [private] |
bool rtabmap::Rtabmap::_statisticLoggedHeaders [private] |
bool rtabmap::Rtabmap::_statisticLogsBufferedInRAM [private] |
std::string rtabmap::Rtabmap::_wDir [private] |
Statistics rtabmap::Rtabmap::statistics_ [private] |