#include <Memory.h>
Public Member Functions | |
bool | addLink (const Link &link, bool addInDatabase=false) |
bool | allNodesInWM () const |
int | cleanup () |
void | close (bool databaseSaved=true, bool postInitClosingEvents=false, const std::string &ouputDatabasePath="") |
Transform | computeIcpTransformMulti (int newId, int oldId, const std::map< int, Transform > &poses, RegistrationInfo *info=0) |
std::map< int, float > | computeLikelihood (const Signature *signature, const std::list< int > &ids) |
Transform | computeTransform (Signature &fromS, Signature &toS, Transform guess, RegistrationInfo *info=0, bool useKnownCorrespondencesIfPossible=false) const |
Transform | computeTransform (int fromId, int toId, Transform guess, RegistrationInfo *info=0, bool useKnownCorrespondencesIfPossible=false) |
void | deleteLocation (int locationId, std::list< int > *deletedWords=0) |
void | dumpDictionary (const char *fileNameRef, const char *fileNameDesc) const |
virtual void | dumpMemory (std::string directory) const |
void | dumpMemoryTree (const char *fileNameTree) const |
virtual void | dumpSignatures (const char *fileNameSign, bool words3D) const |
void | emptyTrash () |
std::list< int > | forget (const std::set< int > &ignoredIds=std::set< int >()) |
void | generateGraph (const std::string &fileName, const std::set< int > &ids=std::set< int >()) |
const std::map< int, std::string > & | getAllLabels () const |
std::multimap< int, Link > | getAllLinks (bool lookInDatabase, bool ignoreNullLinks=true, bool withLandmarks=false) const |
std::set< int > | getAllSignatureIds (bool ignoreChildren=true) const |
int | getDatabaseMemoryUsed () const |
std::string | getDatabaseUrl () const |
std::string | getDatabaseVersion () const |
double | getDbSavingTime () const |
const Feature2D * | getFeature2D () const |
void | getGPS (int id, GPS &gps, Transform &offsetENU, bool lookInDatabase, int maxGraphDepth=0) const |
Transform | getGroundTruthPose (int signatureId, bool lookInDatabase=false) const |
const std::map< int, Transform > & | getGroundTruths () const |
cv::Mat | getImageCompressed (int signatureId) const |
const std::map< int, std::set< int > > & | getLandmarksIndex () const |
const std::map< int, std::set< int > > & | getLandmarksInvertedIndex () const |
int | getLastGlobalLoopClosureId () const |
int | getLastSignatureId () const |
const Signature * | getLastWorkingSignature () const |
std::multimap< int, Link > | getLinks (int signatureId, bool lookInDatabase=false, bool withLandmarks=false) const |
std::multimap< int, Link > | getLoopClosureLinks (int signatureId, bool lookInDatabase=false) const |
int | getMapId (int id, bool lookInDatabase=false) const |
int | getMaxStMemSize () const |
unsigned long | getMemoryUsed () const |
void | getMetricConstraints (const std::set< int > &ids, std::map< int, Transform > &poses, std::multimap< int, Link > &links, bool lookInDatabase=false, bool landmarksAdded=false) |
std::multimap< int, Link > | getNeighborLinks (int signatureId, bool lookInDatabase=false) const |
std::map< int, int > | getNeighborsId (int signatureId, int maxGraphDepth, int maxCheckedInDatabase=-1, bool incrementMarginOnLoop=false, bool ignoreLoopIds=false, bool ignoreIntermediateNodes=false, bool ignoreLocalSpaceLoopIds=false, const std::set< int > &nodesSet=std::set< int >(), double *dbAccessTime=0) const |
std::map< int, float > | getNeighborsIdRadius (int signatureId, float radius, const std::map< int, Transform > &optimizedPoses, int maxGraphDepth) const |
void | getNodeCalibration (int nodeId, std::vector< CameraModel > &models, StereoCameraModel &stereoModel) const |
SensorData | getNodeData (int locationId, bool images, bool scan, bool userData, bool occupancyGrid) const |
bool | getNodeInfo (int signatureId, Transform &odomPose, int &mapId, int &weight, std::string &label, double &stamp, Transform &groundTruth, std::vector< float > &velocity, GPS &gps, EnvSensors &sensors, bool lookInDatabase=false) const |
std::map< int, Link > | getNodesObservingLandmark (int landmarkId, bool lookInDatabase) const |
void | getNodeWordsAndGlobalDescriptors (int nodeId, std::multimap< int, int > &words, std::vector< cv::KeyPoint > &wordsKpts, std::vector< cv::Point3f > &words3, cv::Mat &wordsDescriptors, std::vector< GlobalDescriptor > &globalDescriptors) const |
const std::vector< double > & | getOdomMaxInf () const |
Transform | getOdomPose (int signatureId, bool lookInDatabase=false) const |
virtual const ParametersMap & | getParameters () const |
const Signature * | getSignature (int id) const |
int | getSignatureIdByLabel (const std::string &label, bool lookInDatabase=true) const |
float | getSimilarityThreshold () const |
const std::set< int > & | getStMem () const |
const VWDictionary * | getVWDictionary () const |
std::map< int, int > | getWeights () const |
const std::map< int, double > & | getWorkingMem () const |
int | incrementMapId (std::map< int, int > *reducedIds=0) |
bool | init (const std::string &dbUrl, bool dbOverwritten=false, const ParametersMap ¶meters=ParametersMap(), bool postInitClosingEvents=false) |
bool | isBinDataKept () const |
bool | isGraphReduced () const |
bool | isIDsGenerated () const |
bool | isIncremental () const |
bool | isInLTM (int signatureId) const |
bool | isInSTM (int signatureId) const |
bool | isInWM (int signatureId) const |
bool | isLocalizationDataSaved () const |
bool | isOdomGravityUsed () const |
void | joinTrashThread () |
bool | labelSignature (int id, const std::string &label) |
cv::Mat | load2DMap (float &xMin, float &yMin, float &cellSize) const |
cv::Mat | loadOptimizedMesh (std::vector< std::vector< std::vector< RTABMAP_PCL_INDEX > > > *polygons=0, std::vector< std::vector< Eigen::Vector2f > > *texCoords=0, cv::Mat *textures=0) const |
std::map< int, Transform > | loadOptimizedPoses (Transform *lastlocalizationPose) const |
cv::Mat | loadPreviewImage () const |
Memory (const ParametersMap ¶meters=ParametersMap()) | |
bool | memoryChanged () const |
virtual void | parseParameters (const ParametersMap ¶meters) |
std::set< int > | reactivateSignatures (const std::list< int > &ids, unsigned int maxLoaded, double &timeDbAccess) |
void | removeAllVirtualLinks () |
void | removeLink (int idA, int idB) |
void | removeRawData (int id, bool image=true, bool scan=true, bool userData=true) |
void | removeVirtualLinks (int signatureId) |
void | save2DMap (const cv::Mat &map, float xMin, float yMin, float cellSize) const |
void | saveLocationData (int locationId) |
void | saveOptimizedMesh (const cv::Mat &cloud, const std::vector< std::vector< std::vector< RTABMAP_PCL_INDEX > > > &polygons=std::vector< std::vector< std::vector< RTABMAP_PCL_INDEX > > >(), const std::vector< std::vector< Eigen::Vector2f > > &texCoords=std::vector< std::vector< Eigen::Vector2f > >(), const cv::Mat &textures=cv::Mat()) const |
void | saveOptimizedPoses (const std::map< int, Transform > &optimizedPoses, const Transform &lastlocalizationPose) const |
void | savePreviewImage (const cv::Mat &image) const |
void | saveStatistics (const Statistics &statistics, bool saveWMState) |
bool | setUserData (int id, const cv::Mat &data) |
bool | update (const SensorData &data, Statistics *stats=0) |
bool | update (const SensorData &data, const Transform &pose, const cv::Mat &covariance, const std::vector< float > &velocity=std::vector< float >(), Statistics *stats=0) |
void | updateAge (int signatureId) |
void | updateLink (const Link &link, bool updateInDatabase=false) |
virtual | ~Memory () |
Static Public Attributes | |
static const int | kIdInvalid = 0 |
static const int | kIdStart = 0 |
static const int | kIdVirtual = -1 |
Protected Attributes | |
DBDriver * | _dbDriver |
Private Member Functions | |
Signature * | _getSignature (int id) const |
void | addSignatureToStm (Signature *signature, const cv::Mat &covariance) |
void | addSignatureToWmFromLTM (Signature *signature) |
void | cleanUnusedWords () |
void | clear () |
void | copyData (const Signature *from, Signature *to) |
Signature * | createSignature (const SensorData &data, const Transform &pose, Statistics *stats=0) |
void | disableWordsRef (int signatureId) |
void | enableWordsRef (const std::list< int > &signatureIds) |
int | getNextId () |
int | getNi (int signatureId) const |
std::list< Signature * > | getRemovableSignatures (int count, const std::set< int > &ignoredIds=std::set< int >()) |
const std::map< int, Signature * > & | getSignatures () const |
void | initCountId () |
void | loadDataFromDb (bool postInitClosingEvents) |
void | moveSignatureToWMFromSTM (int id, int *reducedTo=0) |
void | moveToTrash (Signature *s, bool keepLinkedToGraph=true, std::list< int > *deletedWords=0) |
void | preUpdate () |
void | rehearsal (Signature *signature, Statistics *stats=0) |
bool | rehearsalMerge (int oldId, int newId) |
rtabmap::Memory::Memory | ( | const ParametersMap & | parameters = ParametersMap() | ) |
Definition at line 73 of file Memory.cpp.
|
virtual |
Definition at line 517 of file Memory.cpp.
|
private |
Definition at line 1152 of file Memory.cpp.
Definition at line 3202 of file Memory.cpp.
|
private |
Definition at line 883 of file Memory.cpp.
|
private |
Definition at line 1023 of file Memory.cpp.
|
private |
Definition at line 5399 of file Memory.cpp.
int rtabmap::Memory::cleanup | ( | ) |
Definition at line 2007 of file Memory.cpp.
|
private |
Definition at line 1668 of file Memory.cpp.
void rtabmap::Memory::close | ( | bool | databaseSaved = true , |
bool | postInitClosingEvents = false , |
||
const std::string & | ouputDatabasePath = "" |
||
) |
Definition at line 460 of file Memory.cpp.
Transform rtabmap::Memory::computeIcpTransformMulti | ( | int | newId, |
int | oldId, | ||
const std::map< int, Transform > & | poses, | ||
RegistrationInfo * | info = 0 |
||
) |
Definition at line 3004 of file Memory.cpp.
std::map< int, float > rtabmap::Memory::computeLikelihood | ( | const Signature * | signature, |
const std::list< int > & | ids | ||
) |
Compute the likelihood of the signature with some others in the memory. Important: Assuming that all other ids are under 'signature' id. If an error occurs, the result is empty.
Definition at line 1795 of file Memory.cpp.
Transform rtabmap::Memory::computeTransform | ( | Signature & | fromS, |
Signature & | toS, | ||
Transform | guess, | ||
RegistrationInfo * | info = 0 , |
||
bool | useKnownCorrespondencesIfPossible = false |
||
) | const |
Definition at line 2705 of file Memory.cpp.
Transform rtabmap::Memory::computeTransform | ( | int | fromId, |
int | toId, | ||
Transform | guess, | ||
RegistrationInfo * | info = 0 , |
||
bool | useKnownCorrespondencesIfPossible = false |
||
) |
Definition at line 2676 of file Memory.cpp.
Definition at line 4041 of file Memory.cpp.
|
private |
Definition at line 4090 of file Memory.cpp.
void rtabmap::Memory::deleteLocation | ( | int | locationId, |
std::list< int > * | deletedWords = 0 |
||
) |
Definition at line 2570 of file Memory.cpp.
|
private |
Definition at line 5377 of file Memory.cpp.
void rtabmap::Memory::dumpDictionary | ( | const char * | fileNameRef, |
const char * | fileNameDesc | ||
) | const |
Definition at line 3395 of file Memory.cpp.
|
virtual |
Definition at line 3386 of file Memory.cpp.
void rtabmap::Memory::dumpMemoryTree | ( | const char * | fileNameTree | ) | const |
Definition at line 3455 of file Memory.cpp.
|
virtual |
Definition at line 3403 of file Memory.cpp.
void rtabmap::Memory::emptyTrash | ( | ) |
Definition at line 2116 of file Memory.cpp.
|
private |
Definition at line 5422 of file Memory.cpp.
std::list< int > rtabmap::Memory::forget | ( | const std::set< int > & | ignoredIds = std::set<int>() | ) |
Definition at line 1935 of file Memory.cpp.
void rtabmap::Memory::generateGraph | ( | const std::string & | fileName, |
const std::set< int > & | ids = std::set<int>() |
||
) |
Definition at line 4014 of file Memory.cpp.
|
inline |
std::multimap< int, Link > rtabmap::Memory::getAllLinks | ( | bool | lookInDatabase, |
bool | ignoreNullLinks = true , |
||
bool | withLandmarks = false |
||
) | const |
Definition at line 1302 of file Memory.cpp.
std::set< int > rtabmap::Memory::getAllSignatureIds | ( | bool | ignoreChildren = true | ) | const |
Definition at line 1654 of file Memory.cpp.
int rtabmap::Memory::getDatabaseMemoryUsed | ( | ) | const |
Definition at line 1618 of file Memory.cpp.
std::string rtabmap::Memory::getDatabaseUrl | ( | ) | const |
Definition at line 1639 of file Memory.cpp.
std::string rtabmap::Memory::getDatabaseVersion | ( | ) | const |
Definition at line 1629 of file Memory.cpp.
double rtabmap::Memory::getDbSavingTime | ( | ) | const |
Definition at line 1649 of file Memory.cpp.
|
inline |
void rtabmap::Memory::getGPS | ( | int | id, |
GPS & | gps, | ||
Transform & | offsetENU, | ||
bool | lookInDatabase, | ||
int | maxGraphDepth = 0 |
||
) | const |
Definition at line 3815 of file Memory.cpp.
Transform rtabmap::Memory::getGroundTruthPose | ( | int | signatureId, |
bool | lookInDatabase = false |
||
) | const |
Definition at line 3802 of file Memory.cpp.
|
inline |
cv::Mat rtabmap::Memory::getImageCompressed | ( | int | signatureId | ) | const |
Definition at line 3896 of file Memory.cpp.
|
inline |
|
inline |
|
inline |
int rtabmap::Memory::getLastSignatureId | ( | ) | const |
Definition at line 2449 of file Memory.cpp.
const Signature * rtabmap::Memory::getLastWorkingSignature | ( | ) | const |
Definition at line 2454 of file Memory.cpp.
std::multimap< int, Link > rtabmap::Memory::getLinks | ( | int | signatureId, |
bool | lookInDatabase = false , |
||
bool | withLandmarks = false |
||
) | const |
Definition at line 1243 of file Memory.cpp.
std::multimap< int, Link > rtabmap::Memory::getLoopClosureLinks | ( | int | signatureId, |
bool | lookInDatabase = false |
||
) | const |
Definition at line 1203 of file Memory.cpp.
int rtabmap::Memory::getMapId | ( | int | id, |
bool | lookInDatabase = false |
||
) | const |
Definition at line 3776 of file Memory.cpp.
unsigned long rtabmap::Memory::getMemoryUsed | ( | ) | const |
Definition at line 3513 of file Memory.cpp.
void rtabmap::Memory::getMetricConstraints | ( | const std::set< int > & | ids, |
std::map< int, Transform > & | poses, | ||
std::multimap< int, Link > & | links, | ||
bool | lookInDatabase = false , |
||
bool | landmarksAdded = false |
||
) |
Definition at line 5611 of file Memory.cpp.
std::multimap< int, Link > rtabmap::Memory::getNeighborLinks | ( | int | signatureId, |
bool | lookInDatabase = false |
||
) | const |
Definition at line 1162 of file Memory.cpp.
std::map< int, int > rtabmap::Memory::getNeighborsId | ( | int | signatureId, |
int | maxGraphDepth, | ||
int | maxCheckedInDatabase = -1 , |
||
bool | incrementMarginOnLoop = false , |
||
bool | ignoreLoopIds = false , |
||
bool | ignoreIntermediateNodes = false , |
||
bool | ignoreLocalSpaceLoopIds = false , |
||
const std::set< int > & | nodesSet = std::set<int>() , |
||
double * | dbAccessTime = 0 |
||
) | const |
Definition at line 1344 of file Memory.cpp.
std::map< int, float > rtabmap::Memory::getNeighborsIdRadius | ( | int | signatureId, |
float | radius, | ||
const std::map< int, Transform > & | optimizedPoses, | ||
int | maxGraphDepth | ||
) | const |
Definition at line 1521 of file Memory.cpp.
|
private |
Definition at line 1581 of file Memory.cpp.
|
private |
Definition at line 4025 of file Memory.cpp.
void rtabmap::Memory::getNodeCalibration | ( | int | nodeId, |
std::vector< CameraModel > & | models, | ||
StereoCameraModel & | stereoModel | ||
) | const |
Definition at line 3996 of file Memory.cpp.
SensorData rtabmap::Memory::getNodeData | ( | int | locationId, |
bool | images, | ||
bool | scan, | ||
bool | userData, | ||
bool | occupancyGrid | ||
) | const |
Definition at line 3913 of file Memory.cpp.
bool rtabmap::Memory::getNodeInfo | ( | int | signatureId, |
Transform & | odomPose, | ||
int & | mapId, | ||
int & | weight, | ||
std::string & | label, | ||
double & | stamp, | ||
Transform & | groundTruth, | ||
std::vector< float > & | velocity, | ||
GPS & | gps, | ||
EnvSensors & | sensors, | ||
bool | lookInDatabase = false |
||
) | const |
Definition at line 3863 of file Memory.cpp.
std::map< int, Link > rtabmap::Memory::getNodesObservingLandmark | ( | int | landmarkId, |
bool | lookInDatabase | ||
) | const |
Definition at line 2460 of file Memory.cpp.
void rtabmap::Memory::getNodeWordsAndGlobalDescriptors | ( | int | nodeId, |
std::multimap< int, int > & | words, | ||
std::vector< cv::KeyPoint > & | wordsKpts, | ||
std::vector< cv::Point3f > & | words3, | ||
cv::Mat & | wordsDescriptors, | ||
std::vector< GlobalDescriptor > & | globalDescriptors | ||
) | const |
Definition at line 3951 of file Memory.cpp.
|
inline |
Definition at line 3789 of file Memory.cpp.
|
inlinevirtual |
|
private |
Definition at line 2165 of file Memory.cpp.
const Signature * rtabmap::Memory::getSignature | ( | int | id | ) | const |
Definition at line 1147 of file Memory.cpp.
int rtabmap::Memory::getSignatureIdByLabel | ( | const std::string & | label, |
bool | lookInDatabase = true |
||
) | const |
Definition at line 2490 of file Memory.cpp.
|
inlineprivate |
|
inline |
|
inline |
const VWDictionary * rtabmap::Memory::getVWDictionary | ( | ) | const |
Definition at line 1157 of file Memory.cpp.
std::map< int, int > rtabmap::Memory::getWeights | ( | ) | const |
Definition at line 1913 of file Memory.cpp.
|
inline |
int rtabmap::Memory::incrementMapId | ( | std::map< int, int > * | reducedIds = 0 | ) |
Definition at line 1586 of file Memory.cpp.
bool rtabmap::Memory::init | ( | const std::string & | dbUrl, |
bool | dbOverwritten = false , |
||
const ParametersMap & | parameters = ParametersMap() , |
||
bool | postInitClosingEvents = false |
||
) |
Definition at line 151 of file Memory.cpp.
|
private |
|
inline |
|
inline |
|
inline |
|
inline |
void rtabmap::Memory::joinTrashThread | ( | ) |
Definition at line 2124 of file Memory.cpp.
bool rtabmap::Memory::labelSignature | ( | int | id, |
const std::string & | label | ||
) |
Definition at line 2513 of file Memory.cpp.
cv::Mat rtabmap::Memory::load2DMap | ( | float & | xMin, |
float & | yMin, | ||
float & | cellSize | ||
) | const |
Definition at line 2075 of file Memory.cpp.
|
private |
Definition at line 216 of file Memory.cpp.
cv::Mat rtabmap::Memory::loadOptimizedMesh | ( | std::vector< std::vector< std::vector< RTABMAP_PCL_INDEX > > > * | polygons = 0 , |
std::vector< std::vector< Eigen::Vector2f > > * | texCoords = 0 , |
||
cv::Mat * | textures = 0 |
||
) | const |
Definition at line 2100 of file Memory.cpp.
std::map< int, Transform > rtabmap::Memory::loadOptimizedPoses | ( | Transform * | lastlocalizationPose | ) | const |
Definition at line 2058 of file Memory.cpp.
cv::Mat rtabmap::Memory::loadPreviewImage | ( | ) | const |
Definition at line 2041 of file Memory.cpp.
|
private |
Definition at line 1041 of file Memory.cpp.
|
private |
If saveToDatabase=false, deleted words are filled in deletedWords.
Definition at line 2296 of file Memory.cpp.
|
virtual |
Definition at line 532 of file Memory.cpp.
|
private |
Definition at line 745 of file Memory.cpp.
std::set< int > rtabmap::Memory::reactivateSignatures | ( | const std::list< int > & | ids, |
unsigned int | maxLoaded, | ||
double & | timeDbAccess | ||
) |
Definition at line 5534 of file Memory.cpp.
|
private |
Definition at line 3554 of file Memory.cpp.
|
private |
Definition at line 3611 of file Memory.cpp.
void rtabmap::Memory::removeAllVirtualLinks | ( | ) |
Definition at line 3347 of file Memory.cpp.
void rtabmap::Memory::removeLink | ( | int | idA, |
int | idB | ||
) |
Definition at line 2599 of file Memory.cpp.
void rtabmap::Memory::removeRawData | ( | int | id, |
bool | image = true , |
||
bool | scan = true , |
||
bool | userData = true |
||
) |
Definition at line 2662 of file Memory.cpp.
void rtabmap::Memory::removeVirtualLinks | ( | int | signatureId | ) |
Definition at line 3356 of file Memory.cpp.
void rtabmap::Memory::save2DMap | ( | const cv::Mat & | map, |
float | xMin, | ||
float | yMin, | ||
float | cellSize | ||
) | const |
Definition at line 2067 of file Memory.cpp.
void rtabmap::Memory::saveLocationData | ( | int | locationId | ) |
Definition at line 2580 of file Memory.cpp.
void rtabmap::Memory::saveOptimizedMesh | ( | const cv::Mat & | cloud, |
const std::vector< std::vector< std::vector< RTABMAP_PCL_INDEX > > > & | polygons = std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > >() , |
||
const std::vector< std::vector< Eigen::Vector2f > > & | texCoords = std::vector<std::vector<Eigen::Vector2f> >() , |
||
const cv::Mat & | textures = cv::Mat() |
||
) | const |
Definition at line 2084 of file Memory.cpp.
void rtabmap::Memory::saveOptimizedPoses | ( | const std::map< int, Transform > & | optimizedPoses, |
const Transform & | lastlocalizationPose | ||
) | const |
Definition at line 2050 of file Memory.cpp.
void rtabmap::Memory::savePreviewImage | ( | const cv::Mat & | image | ) | const |
Definition at line 2034 of file Memory.cpp.
void rtabmap::Memory::saveStatistics | ( | const Statistics & | statistics, |
bool | saveWMState | ||
) |
Definition at line 2026 of file Memory.cpp.
bool rtabmap::Memory::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 2555 of file Memory.cpp.
bool rtabmap::Memory::update | ( | const SensorData & | data, |
Statistics * | stats = 0 |
||
) |
Definition at line 759 of file Memory.cpp.
bool rtabmap::Memory::update | ( | const SensorData & | data, |
const Transform & | pose, | ||
const cv::Mat & | covariance, | ||
const std::vector< float > & | velocity = std::vector<float>() , |
||
Statistics * | stats = 0 |
||
) |
Definition at line 766 of file Memory.cpp.
void rtabmap::Memory::updateAge | ( | int | signatureId | ) |
Definition at line 1609 of file Memory.cpp.
Definition at line 3287 of file Memory.cpp.
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |