#include <Rtabmap.h>
Public Types | |
enum | VhStrategy { kVhNone, kVhEpipolar, kVhUndef } |
Public Member Functions | |
void | adjustLikelihood (std::map< int, float > &likelihood) const |
void | clearPath (int status) |
void | close (bool databaseSaved=true, const std::string &ouputDatabasePath="") |
bool | computePath (int targetNode, bool global) |
bool | computePath (const Transform &targetPose, float tolerance=-1.0f) |
void | deleteLastLocation () |
int | detectMoreLoopClosures (float clusterRadius=0.5f, float clusterAngle=M_PI/6.0f, int iterations=1, const ProgressState *state=0) |
void | dumpData () const |
void | dumpPrediction () const |
void | exportPoses (const std::string &path, bool optimized, bool global, int format) |
void | generateDOTGraph (const std::string &path, int id=0, int margin=5) |
void | get3DMap (std::map< int, Signature > &signatures, std::map< int, Transform > &poses, std::multimap< int, Link > &constraints, 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, bool optimized, bool global, std::map< int, Signature > *signatures=0) |
int | getHighestHypothesisId () const |
float | getHighestHypothesisValue () const |
const Transform & | getLastLocalizationPose () const |
int | getLastLocationId () const |
double | getLastProcessTime () const |
const std::multimap< int, Link > & | getLocalConstraints () 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 ParametersMap & | getParameters () const |
const std::vector< std::pair< int, Transform > > & | getPath () const |
int | getPathCurrentGoalId () const |
unsigned int | getPathCurrentGoalIndex () const |
unsigned int | getPathCurrentIndex () const |
std::vector< int > | getPathNextNodes () const |
std::vector< std::pair< int, Transform > > | getPathNextPoses () const |
std::map< int, std::map< int, Transform > > | getPaths (std::map< int, Transform > poses, const Transform &target, int maxGraphDepth=0) const |
int | getPathStatus () 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 | isRGBDMode () const |
bool | labelLocation (int id, const std::string &label) |
void | parseParameters (const ParametersMap ¶meters) |
bool | process (const SensorData &data, Transform odomPose, const cv::Mat &odomCovariance=cv::Mat::eye(6, 6, CV_64FC1), const std::vector< float > &odomVelocity=std::vector< float >(), const std::map< std::string, float > &externalStats=std::map< std::string, float >()) |
Main loop of rtabmap. More... | |
bool | process (const SensorData &data, Transform odomPose, float odomLinearVariance, float odomAngularVariance, const std::vector< float > &odomVelocity=std::vector< float >(), const std::map< std::string, float > &externalStats=std::map< std::string, float >()) |
bool | process (const cv::Mat &image, int id=0, const std::map< std::string, float > &externalStats=std::map< std::string, float >()) |
int | refineLinks () |
void | rejectLastLoopClosure () |
void | resetMemory () |
Rtabmap () | |
std::pair< int, float > | selectHypothesis (const std::map< int, float > &posterior, const std::map< int, float > &likelihood) const |
void | setInitialPose (const Transform &initialPose) |
void | setOptimizedPoses (const std::map< int, Transform > &poses) |
void | setTimeThreshold (float maxTimeAllowed) |
bool | setUserData (int id, const cv::Mat &data) |
void | setWorkingDirectory (std::string path) |
int | triggerNewMap () |
virtual | ~Rtabmap () |
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, cv::Mat &covariance, std::multimap< int, Link > *constraints=0, double *error=0, int *iterationsDone=0) const |
std::map< int, Transform > | optimizeGraph (int fromId, const std::set< int > &ids, const std::map< int, Transform > &guessPoses, bool lookInDatabase, cv::Mat &covariance, std::multimap< int, Link > *constraints=0, double *error=0, int *iterationsDone=0) const |
void | setupLogFiles (bool overwrite=false) |
void | updateGoalIndex () |
rtabmap::Rtabmap::Rtabmap | ( | ) |
Definition at line 78 of file Rtabmap.cpp.
|
virtual |
Definition at line 149 of file Rtabmap.cpp.
void rtabmap::Rtabmap::adjustLikelihood | ( | std::map< int, float > & | likelihood | ) | const |
Definition at line 3541 of file Rtabmap.cpp.
void rtabmap::Rtabmap::clearPath | ( | int | status | ) |
Definition at line 4139 of file Rtabmap.cpp.
void rtabmap::Rtabmap::close | ( | bool | databaseSaved = true , |
const std::string & | ouputDatabasePath = "" |
||
) |
Close rtabmap. This will delete rtabmap object if set.
databaseSaved | true=database saved, false=database discarded. |
databasePath | output database file name, ignored if Db/Sqlite3InMemory=false (opened database is then overwritten). |
Definition at line 348 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::computePath | ( | int | targetNode, |
bool | global | ||
) |
Definition at line 4157 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::computePath | ( | const Transform & | targetPose, |
float | tolerance = -1.0f |
||
) |
Definition at line 4274 of file Rtabmap.cpp.
|
private |
void rtabmap::Rtabmap::deleteLastLocation | ( | ) |
Definition at line 3126 of file Rtabmap.cpp.
int rtabmap::Rtabmap::detectMoreLoopClosures | ( | float | clusterRadius = 0.5f , |
float | clusterAngle = M_PI/6.0f , |
||
int | iterations = 1 , |
||
const ProgressState * | state = 0 |
||
) |
Definition at line 3843 of file Rtabmap.cpp.
void rtabmap::Rtabmap::dumpData | ( | ) | const |
Definition at line 3181 of file Rtabmap.cpp.
void rtabmap::Rtabmap::dumpPrediction | ( | ) | const |
Definition at line 3612 of file Rtabmap.cpp.
void rtabmap::Rtabmap::exportPoses | ( | const std::string & | path, |
bool | optimized, | ||
bool | global, | ||
int | format | ||
) |
Definition at line 791 of file Rtabmap.cpp.
|
private |
Definition at line 260 of file Rtabmap.cpp.
void rtabmap::Rtabmap::generateDOTGraph | ( | const std::string & | path, |
int | id = 0 , |
||
int | margin = 5 |
||
) |
Definition at line 759 of file Rtabmap.cpp.
void rtabmap::Rtabmap::get3DMap | ( | std::map< int, Signature > & | signatures, |
std::map< int, Transform > & | poses, | ||
std::multimap< int, Link > & | constraints, | ||
bool | optimized, | ||
bool | global | ||
) | const |
Definition at line 3670 of file Rtabmap.cpp.
std::map< int, Transform > rtabmap::Rtabmap::getForwardWMPoses | ( | int | fromId, |
int | maxNearestNeighbors, | ||
float | radius, | ||
int | maxDiffID | ||
) | const |
Definition at line 3199 of file Rtabmap.cpp.
|
inline |
void rtabmap::Rtabmap::getGraph | ( | std::map< int, Transform > & | poses, |
std::multimap< int, Link > & | constraints, | ||
bool | optimized, | ||
bool | global, | ||
std::map< int, Signature > * | signatures = 0 |
||
) |
Definition at line 3759 of file Rtabmap.cpp.
|
inline |
|
inline |
|
inline |
int rtabmap::Rtabmap::getLastLocationId | ( | ) | const |
Definition at line 528 of file Rtabmap.cpp.
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
int rtabmap::Rtabmap::getPathCurrentGoalId | ( | ) | const |
Definition at line 4445 of file Rtabmap.cpp.
|
inline |
|
inline |
std::vector< int > rtabmap::Rtabmap::getPathNextNodes | ( | ) | const |
Definition at line 4420 of file Rtabmap.cpp.
std::vector< std::pair< int, Transform > > rtabmap::Rtabmap::getPathNextPoses | ( | ) | const |
Definition at line 4395 of file Rtabmap.cpp.
std::map< int, std::map< int, Transform > > rtabmap::Rtabmap::getPaths | ( | std::map< int, Transform > | poses, |
const Transform & | target, | ||
int | maxGraphDepth = 0 |
||
) | const |
Definition at line 3315 of file Rtabmap.cpp.
|
inline |
Transform rtabmap::Rtabmap::getPose | ( | int | locationId | ) | const |
Definition at line 654 of file Rtabmap.cpp.
const Statistics & rtabmap::Rtabmap::getStatistics | ( | ) | const |
Definition at line 631 of file Rtabmap.cpp.
std::set< int > rtabmap::Rtabmap::getSTM | ( | ) | const |
Definition at line 569 of file Rtabmap.cpp.
int rtabmap::Rtabmap::getSTMSize | ( | ) | const |
Definition at line 578 of file Rtabmap.cpp.
int rtabmap::Rtabmap::getTotalMemSize | ( | ) | const |
Definition at line 587 of file Rtabmap.cpp.
std::map< int, int > rtabmap::Rtabmap::getWeights | ( | ) | const |
Definition at line 558 of file Rtabmap.cpp.
std::list< int > rtabmap::Rtabmap::getWM | ( | ) | const |
Definition at line 538 of file Rtabmap.cpp.
int rtabmap::Rtabmap::getWMSize | ( | ) | const |
Definition at line 549 of file Rtabmap.cpp.
std::multimap< int, cv::KeyPoint > rtabmap::Rtabmap::getWords | ( | int | locationId | ) | const |
Definition at line 600 of file Rtabmap.cpp.
|
inline |
void rtabmap::Rtabmap::init | ( | const ParametersMap & | parameters, |
const std::string & | databasePath = "" |
||
) |
Definition at line 282 of file Rtabmap.cpp.
void rtabmap::Rtabmap::init | ( | const std::string & | configFile = "" , |
const std::string & | databasePath = "" |
||
) |
Definition at line 334 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::isIDsGenerated | ( | ) | const |
Definition at line 622 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::isInSTM | ( | int | locationId | ) | const |
Definition at line 613 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::labelLocation | ( | int | id, |
const std::string & | label | ||
) |
Definition at line 719 of file Rtabmap.cpp.
|
private |
Definition at line 3378 of file Rtabmap.cpp.
|
private |
Definition at line 3424 of file Rtabmap.cpp.
void rtabmap::Rtabmap::parseParameters | ( | const ParametersMap & | parameters | ) |
Definition at line 405 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::process | ( | const SensorData & | data, |
Transform | odomPose, | ||
const cv::Mat & | odomCovariance = cv::Mat::eye(6,6,CV_64FC1) , |
||
const std::vector< float > & | odomVelocity = std::vector<float>() , |
||
const std::map< std::string, float > & | externalStats = std::map<std::string, float>() |
||
) |
Main loop of rtabmap.
data | Sensor data to process. |
odomPose | Odometry pose, should be non-null for RGB-D SLAM mode. |
covariance | Odometry covariance. |
externalStats | External statistics to be saved in the database for convenience |
Definition at line 897 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::process | ( | const SensorData & | data, |
Transform | odomPose, | ||
float | odomLinearVariance, | ||
float | odomAngularVariance, | ||
const std::vector< float > & | odomVelocity = std::vector<float>() , |
||
const std::map< std::string, float > & | externalStats = std::map<std::string, float>() |
||
) |
Definition at line 875 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::process | ( | const cv::Mat & | image, |
int | id = 0 , |
||
const std::map< std::string, float > & | externalStats = std::map<std::string, float>() |
||
) |
Definition at line 868 of file Rtabmap.cpp.
int rtabmap::Rtabmap::refineLinks | ( | ) |
Definition at line 4093 of file Rtabmap.cpp.
void rtabmap::Rtabmap::rejectLastLoopClosure | ( | ) |
Definition at line 3068 of file Rtabmap.cpp.
void rtabmap::Rtabmap::resetMemory | ( | ) |
Definition at line 829 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::setInitialPose | ( | const Transform & | initialPose | ) |
Definition at line 667 of file Rtabmap.cpp.
void rtabmap::Rtabmap::setOptimizedPoses | ( | const std::map< int, Transform > & | poses | ) |
Definition at line 3176 of file Rtabmap.cpp.
void rtabmap::Rtabmap::setTimeThreshold | ( | float | maxTimeAllowed | ) |
Definition at line 3026 of file Rtabmap.cpp.
|
private |
Definition at line 154 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::setUserData | ( | int | id, |
const cv::Mat & | data | ||
) |
Set user data. Detect automatically if raw or compressed. If raw, the data is compressed too. A matrix of type CV_8UC1 with 1 row is considered as compressed. If you have one dimension unsigned 8 bits raw data, make sure to transpose it (to have multiple rows instead of multiple columns) in order to be detected as not compressed.
Definition at line 739 of file Rtabmap.cpp.
void rtabmap::Rtabmap::setWorkingDirectory | ( | std::string | path | ) |
Definition at line 3041 of file Rtabmap.cpp.
int rtabmap::Rtabmap::triggerNewMap | ( | ) |
Definition at line 685 of file Rtabmap.cpp.
|
private |
Definition at line 4455 of file Rtabmap.cpp.
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |