#include <Rtabmap.h>
Public Types | |
enum | VhStrategy { kVhNone, kVhEpipolar, kVhUndef } |
Public Member Functions | |
bool | addLink (const Link &link) |
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, bool intraSession=true, bool interSession=true, 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) |
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, bool withImages=false, bool withScan=false, bool withUserData=false, bool withGrid=false, bool withWords=true, bool withGlobalDescriptors=true) const |
int | getHighestHypothesisId () const |
float | getHighestHypothesisValue () const |
cv::Mat | getInformation (const cv::Mat &covariance) 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 |
std::map< int, Transform > | getNodesInRadius (const Transform &pose, float radius) |
std::map< int, Transform > | getNodesInRadius (int nodeId, float radius) |
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 (const std::map< int, Transform > &poses, const Transform &target, int maxGraphDepth=0) const |
int | getPathStatus () const |
const Transform & | getPathTransformToGoal () const |
Transform | getPose (int locationId) const |
Signature | getSignatureCopy (int id, bool images, bool scan, bool userData, bool occupancyGrid, bool withWords, bool withGlobalDescriptors) 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 |
const std::string & | getWorkingDir () const |
void | init (const ParametersMap ¶meters, const std::string &databasePath="", bool loadDatabaseParameters=false) |
void | init (const std::string &configFile="", const std::string &databasePath="", bool loadDatabaseParameters=false) |
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 () | |
RTABMAP_DEPRECATED (void get3DMap(std::map< int, Signature > &signatures, std::map< int, Transform > &poses, std::multimap< int, Link > &constraints, bool optimized, bool global) const,"Use getGraph() instead with withImages=true, withScan=true, withUserData=true and withGrid=true.") | |
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 79 of file Rtabmap.cpp.
|
virtual |
Definition at line 159 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::addLink | ( | const Link & | link | ) |
Definition at line 4974 of file Rtabmap.cpp.
void rtabmap::Rtabmap::adjustLikelihood | ( | std::map< int, float > & | likelihood | ) | const |
Definition at line 4324 of file Rtabmap.cpp.
void rtabmap::Rtabmap::clearPath | ( | int | status | ) |
Definition at line 5212 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 392 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::computePath | ( | int | targetNode, |
bool | global | ||
) |
Definition at line 5230 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::computePath | ( | const Transform & | targetPose, |
float | tolerance = -1.0f |
||
) |
Definition at line 5375 of file Rtabmap.cpp.
|
private |
void rtabmap::Rtabmap::deleteLastLocation | ( | ) |
Definition at line 3928 of file Rtabmap.cpp.
int rtabmap::Rtabmap::detectMoreLoopClosures | ( | float | clusterRadius = 0.5f , |
float | clusterAngle = M_PI/6.0f , |
||
int | iterations = 1 , |
||
bool | intraSession = true , |
||
bool | interSession = true , |
||
const ProgressState * | state = 0 |
||
) |
Definition at line 4629 of file Rtabmap.cpp.
void rtabmap::Rtabmap::dumpData | ( | ) | const |
Definition at line 3983 of file Rtabmap.cpp.
void rtabmap::Rtabmap::dumpPrediction | ( | ) | const |
Definition at line 4395 of file Rtabmap.cpp.
void rtabmap::Rtabmap::exportPoses | ( | const std::string & | path, |
bool | optimized, | ||
bool | global, | ||
int | format | ||
) |
Definition at line 858 of file Rtabmap.cpp.
|
private |
Definition at line 270 of file Rtabmap.cpp.
void rtabmap::Rtabmap::generateDOTGraph | ( | const std::string & | path, |
int | id = 0 , |
||
int | margin = 5 |
||
) |
Definition at line 826 of file Rtabmap.cpp.
std::map< int, Transform > rtabmap::Rtabmap::getForwardWMPoses | ( | int | fromId, |
int | maxNearestNeighbors, | ||
float | radius, | ||
int | maxDiffID | ||
) | const |
Definition at line 4001 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 , |
||
bool | withImages = false , |
||
bool | withScan = false , |
||
bool | withUserData = false , |
||
bool | withGrid = false , |
||
bool | withWords = true , |
||
bool | withGlobalDescriptors = true |
||
) | const |
Definition at line 4543 of file Rtabmap.cpp.
|
inline |
|
inline |
cv::Mat rtabmap::Rtabmap::getInformation | ( | const cv::Mat & | covariance | ) | const |
Definition at line 5192 of file Rtabmap.cpp.
|
inline |
int rtabmap::Rtabmap::getLastLocationId | ( | ) | const |
Definition at line 609 of file Rtabmap.cpp.
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
|
inline |
std::map< int, Transform > rtabmap::Rtabmap::getNodesInRadius | ( | const Transform & | pose, |
float | radius | ||
) |
Definition at line 4609 of file Rtabmap.cpp.
std::map< int, Transform > rtabmap::Rtabmap::getNodesInRadius | ( | int | nodeId, |
float | radius | ||
) |
Definition at line 4614 of file Rtabmap.cpp.
|
inline |
|
inline |
int rtabmap::Rtabmap::getPathCurrentGoalId | ( | ) | const |
Definition at line 5554 of file Rtabmap.cpp.
|
inline |
|
inline |
std::vector< int > rtabmap::Rtabmap::getPathNextNodes | ( | ) | const |
Definition at line 5529 of file Rtabmap.cpp.
std::vector< std::pair< int, Transform > > rtabmap::Rtabmap::getPathNextPoses | ( | ) | const |
Definition at line 5504 of file Rtabmap.cpp.
std::map< int, std::map< int, Transform > > rtabmap::Rtabmap::getPaths | ( | const std::map< int, Transform > & | poses, |
const Transform & | target, | ||
int | maxGraphDepth = 0 |
||
) | const |
Definition at line 4117 of file Rtabmap.cpp.
|
inline |
Transform rtabmap::Rtabmap::getPose | ( | int | locationId | ) | const |
Definition at line 704 of file Rtabmap.cpp.
Signature rtabmap::Rtabmap::getSignatureCopy | ( | int | id, |
bool | images, | ||
bool | scan, | ||
bool | userData, | ||
bool | occupancyGrid, | ||
bool | withWords, | ||
bool | withGlobalDescriptors | ||
) | const |
Definition at line 4453 of file Rtabmap.cpp.
const Statistics & rtabmap::Rtabmap::getStatistics | ( | ) | const |
Definition at line 699 of file Rtabmap.cpp.
std::set< int > rtabmap::Rtabmap::getSTM | ( | ) | const |
Definition at line 650 of file Rtabmap.cpp.
int rtabmap::Rtabmap::getSTMSize | ( | ) | const |
Definition at line 659 of file Rtabmap.cpp.
int rtabmap::Rtabmap::getTotalMemSize | ( | ) | const |
Definition at line 668 of file Rtabmap.cpp.
std::map< int, int > rtabmap::Rtabmap::getWeights | ( | ) | const |
Definition at line 639 of file Rtabmap.cpp.
std::list< int > rtabmap::Rtabmap::getWM | ( | ) | const |
Definition at line 619 of file Rtabmap.cpp.
int rtabmap::Rtabmap::getWMSize | ( | ) | const |
Definition at line 630 of file Rtabmap.cpp.
|
inline |
void rtabmap::Rtabmap::init | ( | const ParametersMap & | parameters, |
const std::string & | databasePath = "" , |
||
bool | loadDatabaseParameters = false |
||
) |
Initialize Rtabmap with parameters and a database
parameters | Parameters overriding default parameters and database parameters ( |
databasePath | The database input/output path. If not set, an empty database is used in RAM. If set and the file doesn't exist, it will be created empty. If the database exists, nodes and vocabulary will be loaded in working memory. |
loadDatabaseParameters | If an existing database is used ( |
Definition at line 292 of file Rtabmap.cpp.
void rtabmap::Rtabmap::init | ( | const std::string & | configFile = "" , |
const std::string & | databasePath = "" , |
||
bool | loadDatabaseParameters = false |
||
) |
Initialize Rtabmap with parameters from a configuration file and a database
configFile | Configuration file (*.ini) overriding default parameters and database parameters ( |
databasePath | The database input/output path. If not set, an empty database is used in RAM. If set and the file doesn't exist, it will be created empty. If the database exists, nodes and vocabulary will be loaded in working memory. |
loadDatabaseParameters | If an existing database is used ( |
Definition at line 378 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::isIDsGenerated | ( | ) | const |
Definition at line 690 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::isInSTM | ( | int | locationId | ) | const |
Definition at line 681 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::labelLocation | ( | int | id, |
const std::string & | label | ||
) |
Definition at line 786 of file Rtabmap.cpp.
|
private |
Definition at line 4199 of file Rtabmap.cpp.
|
private |
Definition at line 4245 of file Rtabmap.cpp.
void rtabmap::Rtabmap::parseParameters | ( | const ParametersMap & | parameters | ) |
Definition at line 469 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 992 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 970 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 963 of file Rtabmap.cpp.
int rtabmap::Rtabmap::refineLinks | ( | ) |
Definition at line 4928 of file Rtabmap.cpp.
void rtabmap::Rtabmap::rejectLastLoopClosure | ( | ) |
Definition at line 3870 of file Rtabmap.cpp.
void rtabmap::Rtabmap::resetMemory | ( | ) |
Definition at line 897 of file Rtabmap.cpp.
rtabmap::Rtabmap::RTABMAP_DEPRECATED | ( | void get3DMap(std::map< int, Signature > &signatures, std::map< int, Transform > &poses, std::multimap< int, Link > &constraints, bool optimized, bool global) | const, |
"Use getGraph() instead with | withImages = true , |
||
withScan | = true |
||
) |
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 709 of file Rtabmap.cpp.
void rtabmap::Rtabmap::setOptimizedPoses | ( | const std::map< int, Transform > & | poses | ) |
Definition at line 3978 of file Rtabmap.cpp.
void rtabmap::Rtabmap::setTimeThreshold | ( | float | maxTimeAllowed | ) |
Definition at line 3828 of file Rtabmap.cpp.
|
private |
Definition at line 164 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 806 of file Rtabmap.cpp.
void rtabmap::Rtabmap::setWorkingDirectory | ( | std::string | path | ) |
Definition at line 3843 of file Rtabmap.cpp.
int rtabmap::Rtabmap::triggerNewMap | ( | ) |
Definition at line 737 of file Rtabmap.cpp.
|
private |
Definition at line 5564 of file Rtabmap.cpp.
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |