#include <Rtabmap.h>
Public Types | |
enum | VhStrategy { kVhNone, kVhEpipolar, kVhUndef } |
Public Member Functions | |
bool | addLink (const Link &link) |
void | addNodesToRepublish (const std::vector< int > &ids) |
void | adjustLikelihood (std::map< int, float > &likelihood) const |
int | cleanupLocalGrids (const std::map< int, Transform > &mapPoses, const cv::Mat &map, float xMin, float yMin, float cellSize, int cropRadius=1, bool filterScans=false) |
void | clearPath (int status) |
void | close (bool databaseSaved=true, const std::string &ouputDatabasePath="") |
bool | computePath (const Transform &targetPose, float tolerance=-1.0f) |
bool | computePath (int targetNode, bool global) |
void | deleteLastLocation () |
int | detectMoreLoopClosures (float clusterRadiusMax=0.5f, float clusterAngle=M_PI/6.0f, int iterations=1, bool intraSession=true, bool interSession=true, const ProgressState *state=0, float clusterRadiusMin=0.0f) |
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) |
RTABMAP_DEPRECATED 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, 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 |
int | getMemoryThreshold () const |
std::map< int, Transform > | getNodesInRadius (const Transform &pose, float radius, int k=0, std::map< int, float > *distsSqr=0) |
std::map< int, Transform > | getNodesInRadius (int nodeId, float radius, int k=0, std::map< int, float > *distsSqr=0) |
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 |
bool | globalBundleAdjustment (int optimizerType=1, bool rematchFeatures=true, int iterations=0, float pixelVariance=0.0f) |
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 cv::Mat &image, int id=0, const std::map< std::string, float > &externalStats=std::map< std::string, float >()) |
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 >()) |
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 | setMemoryThreshold (int maxMemoryAllowed) |
void | setOptimizedPoses (const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints) |
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 | createGlobalScanMap () |
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 94 of file Rtabmap.cpp.
|
virtual |
Definition at line 187 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::addLink | ( | const Link & | link | ) |
Definition at line 6057 of file Rtabmap.cpp.
void rtabmap::Rtabmap::addNodesToRepublish | ( | const std::vector< int > & | ids | ) |
Definition at line 6460 of file Rtabmap.cpp.
Definition at line 5260 of file Rtabmap.cpp.
int rtabmap::Rtabmap::cleanupLocalGrids | ( | const std::map< int, Transform > & | mapPoses, |
const cv::Mat & | map, | ||
float | xMin, | ||
float | yMin, | ||
float | cellSize, | ||
int | cropRadius = 1 , |
||
bool | filterScans = false |
||
) |
Definition at line 5988 of file Rtabmap.cpp.
void rtabmap::Rtabmap::clearPath | ( | int | status | ) |
Definition at line 6480 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 463 of file Rtabmap.cpp.
Definition at line 6643 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::computePath | ( | int | targetNode, |
bool | global | ||
) |
Definition at line 6498 of file Rtabmap.cpp.
|
private |
|
private |
Definition at line 7095 of file Rtabmap.cpp.
void rtabmap::Rtabmap::deleteLastLocation | ( | ) |
Definition at line 4849 of file Rtabmap.cpp.
int rtabmap::Rtabmap::detectMoreLoopClosures | ( | float | clusterRadiusMax = 0.5f , |
float | clusterAngle = M_PI/6.0f , |
||
int | iterations = 1 , |
||
bool | intraSession = true , |
||
bool | interSession = true , |
||
const ProgressState * | state = 0 , |
||
float | clusterRadiusMin = 0.0f |
||
) |
Definition at line 5595 of file Rtabmap.cpp.
void rtabmap::Rtabmap::dumpData | ( | ) | const |
Definition at line 4905 of file Rtabmap.cpp.
void rtabmap::Rtabmap::dumpPrediction | ( | ) | const |
Definition at line 5331 of file Rtabmap.cpp.
void rtabmap::Rtabmap::exportPoses | ( | const std::string & | path, |
bool | optimized, | ||
bool | global, | ||
int | format | ||
) |
Definition at line 1024 of file Rtabmap.cpp.
|
private |
Definition at line 298 of file Rtabmap.cpp.
void rtabmap::Rtabmap::generateDOTGraph | ( | const std::string & | path, |
int | id = 0 , |
||
int | margin = 5 |
||
) |
Definition at line 992 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 5468 of file Rtabmap.cpp.
std::map< int, Transform > rtabmap::Rtabmap::getForwardWMPoses | ( | int | fromId, |
int | maxNearestNeighbors, | ||
float | radius, | ||
int | maxDiffID | ||
) | const |
Definition at line 4923 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 5479 of file Rtabmap.cpp.
|
inline |
|
inline |
Definition at line 6440 of file Rtabmap.cpp.
|
inline |
int rtabmap::Rtabmap::getLastLocationId | ( | ) | const |
Definition at line 759 of file Rtabmap.cpp.
|
inline |
|
inline |
|
inline |
|
inline |
std::map< int, Transform > rtabmap::Rtabmap::getNodesInRadius | ( | const Transform & | pose, |
float | radius, | ||
int | k = 0 , |
||
std::map< int, float > * | distsSqr = 0 |
||
) |
Definition at line 5551 of file Rtabmap.cpp.
std::map< int, Transform > rtabmap::Rtabmap::getNodesInRadius | ( | int | nodeId, |
float | radius, | ||
int | k = 0 , |
||
std::map< int, float > * | distsSqr = 0 |
||
) |
Definition at line 5564 of file Rtabmap.cpp.
|
inline |
int rtabmap::Rtabmap::getPathCurrentGoalId | ( | ) | const |
Definition at line 6822 of file Rtabmap.cpp.
|
inline |
|
inline |
std::vector< int > rtabmap::Rtabmap::getPathNextNodes | ( | ) | const |
Definition at line 6797 of file Rtabmap.cpp.
Definition at line 6772 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 5039 of file Rtabmap.cpp.
|
inline |
Definition at line 854 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 5389 of file Rtabmap.cpp.
const Statistics & rtabmap::Rtabmap::getStatistics | ( | ) | const |
Definition at line 849 of file Rtabmap.cpp.
Definition at line 800 of file Rtabmap.cpp.
int rtabmap::Rtabmap::getSTMSize | ( | ) | const |
Definition at line 809 of file Rtabmap.cpp.
int rtabmap::Rtabmap::getTotalMemSize | ( | ) | const |
Definition at line 818 of file Rtabmap.cpp.
Definition at line 789 of file Rtabmap.cpp.
std::list< int > rtabmap::Rtabmap::getWM | ( | ) | const |
Definition at line 769 of file Rtabmap.cpp.
int rtabmap::Rtabmap::getWMSize | ( | ) | const |
Definition at line 780 of file Rtabmap.cpp.
|
inline |
bool rtabmap::Rtabmap::globalBundleAdjustment | ( | int | optimizerType = 1 , |
bool | rematchFeatures = true , |
||
int | iterations = 0 , |
||
float | pixelVariance = 0.0f |
||
) |
Definition at line 5929 of file Rtabmap.cpp.
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 320 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 449 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::isIDsGenerated | ( | ) | const |
Definition at line 840 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::isInSTM | ( | int | locationId | ) | const |
Definition at line 831 of file Rtabmap.cpp.
bool rtabmap::Rtabmap::labelLocation | ( | int | id, |
const std::string & | label | ||
) |
Definition at line 936 of file Rtabmap.cpp.
|
private |
Definition at line 5121 of file Rtabmap.cpp.
|
private |
Definition at line 5167 of file Rtabmap.cpp.
void rtabmap::Rtabmap::parseParameters | ( | const ParametersMap & | parameters | ) |
Definition at line 545 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 1148 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 1177 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 1155 of file Rtabmap.cpp.
int rtabmap::Rtabmap::refineLinks | ( | ) |
Definition at line 6011 of file Rtabmap.cpp.
void rtabmap::Rtabmap::rejectLastLoopClosure | ( | ) |
Definition at line 4791 of file Rtabmap.cpp.
void rtabmap::Rtabmap::resetMemory | ( | ) |
Definition at line 1063 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 859 of file Rtabmap.cpp.
void rtabmap::Rtabmap::setMemoryThreshold | ( | int | maxMemoryAllowed | ) |
Definition at line 4752 of file Rtabmap.cpp.
void rtabmap::Rtabmap::setOptimizedPoses | ( | const std::map< int, Transform > & | poses, |
const std::multimap< int, Link > & | constraints | ||
) |
Definition at line 4899 of file Rtabmap.cpp.
void rtabmap::Rtabmap::setTimeThreshold | ( | float | maxTimeAllowed | ) |
Definition at line 4738 of file Rtabmap.cpp.
|
private |
Definition at line 192 of file Rtabmap.cpp.
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 972 of file Rtabmap.cpp.
void rtabmap::Rtabmap::setWorkingDirectory | ( | std::string | path | ) |
Definition at line 4763 of file Rtabmap.cpp.
int rtabmap::Rtabmap::triggerNewMap | ( | ) |
Definition at line 887 of file Rtabmap.cpp.
|
private |
Definition at line 6832 of file Rtabmap.cpp.
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |