i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-03 16:47:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added doc for Rtabmap and Memory classes

Browse Source
This commit is contained in:
matlabbe
2026-05-23 19:01:25 -07:00
parent 5aff0721e9
commit 5786a8796d
2 changed files with 1185 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Memory.h
+483 -11
View File
@@ -61,47 +61,203 @@ class LocalGridMaker;
class MarkerDetector;
class GlobalDescriptorExtractor;
/**
* @class Memory
* @brief Three-tiered memory management (STM, WM, LTM) for RTAB-Map.
*
* Memory is the core map data structure used by @ref Rtabmap. It stores observations
* as @ref Signature nodes connected by @ref Link edges and orchestrates their lifecycle
* across three tiers:
*
* - **Short-Term Memory (STM)**: most recently added signatures, kept at a fixed size
* (see @ref Parameters::kMemSTMSize()). Used to delay recently observed places before
* they become candidates for loop closure.
* - **Working Memory (WM)**: signatures available for loop-closure likelihood
* computation in the current iteration. Older signatures are transferred from WM
* to LTM by @ref forget() to bound iteration time.
* - **Long-Term Memory (LTM)**: persisted in the database via @ref DBDriver. Signatures
* can be brought back to WM with @ref reactivateSignatures() when their neighbors still
* in WM are good loop-closure candidates.
*
* The class also owns a visual word dictionary (@ref VWDictionary), feature extractor
* (@ref Feature2D) and registration pipelines (@ref Registration, @ref RegistrationVis,
* @ref RegistrationIcp) used to compute relative transforms between signatures.
*
* Typical iteration: @ref update() adds a new @ref SensorData as a @ref Signature in
* STM; @ref computeLikelihood() scores it against WM; the @ref Rtabmap caller decides
* on loop closures with @ref BayesFilter; @ref cleanup() drops bad signatures;
* @ref forget() transfers oldest WM signatures to LTM and @ref reactivateSignatures()
* pulls relevant ones back.
*
* @see Signature
* @see DBDriver
* @see VWDictionary
* @see Rtabmap
*/
class RTABMAP_CORE_EXPORT Memory
{
public:
/** @brief First valid signature id assigned by @ref getNextId() (positive integer). */
static const int kIdStart;
/** @brief Reserved id for the "virtual place" used by the Bayes filter (negative). */
static const int kIdVirtual;
/** @brief Sentinel value indicating an invalid signature id (zero). */
static const int kIdInvalid;
public:
/**
* @brief Constructs a Memory instance with the given parameters.
*
* The database is not opened here; call @ref init() to open or create a database
* and load persisted state. @p parameters may include any key from @ref Parameters
* (memory, keypoint, registration, etc.); missing keys fall back to defaults.
*/
Memory(const ParametersMap & parameters = ParametersMap());
virtual ~Memory();
/**
* @brief Re-parses parameters and propagates them to owned sub-objects.
*
* Forwards the relevant subset to @ref VWDictionary, @ref Feature2D, the registration
* pipelines and the database driver. Safe to call at runtime to change settings.
*/
virtual void parseParameters(const ParametersMap & parameters);
/** @return The most recent parameter map applied via the constructor or @ref parseParameters(). */
virtual const ParametersMap & getParameters() const {return parameters_;}
/**
* @brief Adds a sensor observation to the map (overload without odometry pose).
*
* Equivalent to calling the full @ref update() with an identity pose and empty covariance.
* The new signature is added to STM; oldest STM entries are promoted to WM as needed.
*
* @param data Sensor data (images, scan, user data, odometry features) for this frame.
* @param stats Optional output statistics receiver for timing/diagnostic values.
* @return True if a signature was successfully created and added, false otherwise.
*/
bool update(const SensorData & data,
Statistics * stats = 0);
/**
* @brief Adds a sensor observation with odometry pose and velocity to the map.
*
* Creates a new @ref Signature, extracts visual words, links it to the previous
* STM signature with a neighbor link, runs rehearsal against STM and promotes
* the oldest STM signature to WM if STM is full.
*
* @param data Sensor data (images, scan, user data, odometry features).
* @param pose Odometry pose at this frame (null if odometry not used).
* @param covariance 6x6 odometry covariance (or empty if null odometry is provided).
* @param velocity Optional 6-vector (vx, vy, vz, vroll, vpitch, vyaw).
* @param stats Optional output statistics receiver.
* @return True on success, false if signature creation failed.
*/
bool update(const SensorData & data,
const Transform & pose,
const cv::Mat & covariance,
const std::vector<float> & velocity = std::vector<float>(), // vx,vy,vz,vroll,vpitch,vyaw
Statistics * stats = 0);
/**
* @brief Opens or creates a database and loads existing state into WM.
*
* @param dbUrl Path to the database file (empty for in-memory).
* @param dbOverwritten If true, deletes the existing file before opening.
* @param parameters Optional parameter override applied before loading.
* @param postInitClosingEvents If true, posts @ref RtabmapEventInit events for
* progress reporting (used by GUI).
* @return True on success, false if the database could not be opened.
*/
bool init(const std::string & dbUrl,
bool dbOverwritten = false,
const ParametersMap & parameters = ParametersMap(),
bool postInitClosingEvents = false);
/**
* @brief Flushes pending data and closes the database connection.
*
* @param databaseSaved If true, persists STM/WM signatures and statistics before closing.
* If false, in-memory state is discarded.
* @param postInitClosingEvents If true, posts progress events while closing.
* @param ouputDatabasePath If non-empty, the database is copied to this path on close. If a
* database on disk was initially created/loaded on a different path, it will be updated with latest
* changes and renamed to the output path.
*/
void close(bool databaseSaved = true, bool postInitClosingEvents = false, const std::string & ouputDatabasePath = "");
/**
* @brief Computes loop-closure likelihood of @p signature against a set of WM ids.
*
* Compares visual words (tf-idf if enabled) between @p signature and each id in
* @p ids and returns a normalized likelihood per id.
*
* @param signature Query signature (typically the last added one).
* @param ids Candidate signature ids in working memory.
* @return Map from id to likelihood score.
*/
std::map<int, float> computeLikelihood(const Signature * signature,
const std::list<int> & ids);
/**
* @brief Starts a new map id, breaking session continuity (e.g. after localization loss).
*
* @param reducedIds If non-null, populated with id remappings produced by graph reduction
* triggered by the new map.
* @return The new map id (auto-incremented).
*/
int incrementMapId(std::map<int, int> * reducedIds = 0);
/**
* @brief Refreshes the age of @p signatureId in working memory, marking it as recent.
*
* Used to keep loop-closure hypotheses active so they are not transferred to LTM
* during the next @ref forget() call.
*/
void updateAge(int signatureId);
/**
* @brief Transfers oldest signatures from WM to LTM to respect memory and/or time limits.
*
* The number of signatures removed depends on the visual word dictionary growth
* since the previous iteration (including retrieved signatures). We remove signatures
* until the number of visual words transferred is greater than the number of retrieved
* ones on the last iteration. In the case that signatures don't have visual words (e.g., lidar-only mapping),
* we remove at least one more signature than the total of signatures added/retrieved in the
* previous iteration.
*
* @param ignoredIds Signatures that must not be transferred (e.g. STM, retrieved ids, on the planned path).
* @return Ids of signatures moved to LTM, in transfer order.
*/
std::list<int> forget(const std::set<int> & ignoredIds = std::set<int>());
/**
* @brief Reloads signatures from LTM into WM.
*
* @param ids Candidate ids; those already in WM/STM are ignored.
* @param maxLoaded Hard cap on number of ids actually loaded (0 = unlimited).
* @param timeDbAccess Output: time spent in the database driver (seconds).
* @return Ids effectively brought back to WM.
*/
std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);
/**
* @brief Drops the last signature if flagged as bad, or any signature in localization mode.
* @return Id of the removed signature, or 0 if none was removed.
*/
int cleanup();
/** @brief Persists @p statistics to the database; @p saveWMState records the WM id list. */
void saveStatistics(const Statistics & statistics, bool saveWMState);
/** @brief Stores a preview image (typically a thumbnail of the last frame) in the database. */
void savePreviewImage(const cv::Mat & image) const;
/** @brief Loads the preview image previously written by @ref savePreviewImage(). */
cv::Mat loadPreviewImage() const;
/** @brief Persists an optimized pose graph and the last localization pose for next session. */
void saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
/** @brief Loads optimized poses previously written by @ref saveOptimizedPoses(). */
std::map<int, Transform> loadOptimizedPoses(Transform * lastlocalizationPose) const;
/** @brief Persists a 2D occupancy grid (origin @p xMin, @p yMin and resolution @p cellSize). */
void save2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize) const;
/** @brief Loads the 2D occupancy grid previously written by @ref save2DMap(). */
cv::Mat load2DMap(float & xMin, float & yMin, float & cellSize) const;
/**
* @brief Persists an optimized textured/colored mesh to the database.
* @param cloud Point cloud (XYZRGB) of vertices.
* @param polygons Per-texture list of polygons; each polygon is a list of vertex indices.
* @param texCoords Per-texture list of UV coords matching @p polygons.
* @param textures Concatenated texture images (square, equal-sized).
*/
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> > >(), // Textures -> polygons -> vertices
@@ -111,6 +267,7 @@ public:
const std::vector<std::vector<Eigen::Vector2f> > & texCoords = std::vector<std::vector<Eigen::Vector2f> >(), // Textures -> uv coords for each vertex of the polygons
#endif
const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size)
/** @brief Loads the optimized mesh previously written by @ref saveOptimizedMesh(). */
cv::Mat loadOptimizedMesh(
std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > * polygons = 0,
#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
@@ -119,12 +276,39 @@ public:
std::vector<std::vector<Eigen::Vector2f> > * texCoords = 0,
#endif
cv::Mat * textures = 0) const;
/** @brief Forces the database driver to flush any queued signature/word saves. */
void emptyTrash();
/** @brief Blocks until the asynchronous database write thread has finished pending work. */
void joinTrashThread();
/**
* @brief Adds a graph link between two signatures.
* @param link Link to add (type, transform, covariance).
* @param addInDatabase If true, the link is also added when one of the ids is only in LTM.
* @return True if the link was added, false on conflict or missing nodes.
*/
bool addLink(const Link & link, bool addInDatabase = false);
/** @brief Replaces an existing link with @p link (same endpoints and type). */
void updateLink(const Link & link, bool updateInDatabase = false);
/** @brief Removes every virtual link in WM. */
void removeAllVirtualLinks();
/** @brief Removes virtual links attached to @p signatureId. */
void removeVirtualLinks(int signatureId);
/**
* @brief Breadth-first walk of the pose graph from @p signatureId.
*
* Visits neighbor and (optionally) loop-closure neighbors up to @p maxGraphDepth.
*
* @param signatureId Starting node.
* @param maxGraphDepth Maximum graph distance (0 = infinite graph depth).
* @param maxCheckedInDatabase Cap on LTM look-ups (-1 = unlimited, 0 = WM only).
* @param incrementMarginOnLoop If true, loop-closure links count toward depth.
* @param ignoreLoopIds If true, loop-closure neighbors are not traversed.
* @param ignoreIntermediateNodes If true, weight==-1 intermediate nodes are skipped.
* @param ignoreLocalSpaceLoopIds If true, only global loop closures are traversed.
* @param nodesSet If non-empty, traversal is constrained to these ids.
* @param dbAccessTime Output: time spent in database access (seconds).
* @return Map from visited node id to graph depth, including @p signatureId (with graph depth of 0)
*/
std::map<int, int> getNeighborsId(
int signatureId,
int maxGraphDepth,
@@ -135,59 +319,171 @@ public:
bool ignoreLocalSpaceLoopIds = false,
const std::set<int> & nodesSet = std::set<int>(),
double * dbAccessTime = 0) const;
/**
* @brief Returns neighbor ids within a Euclidean radius using optimized poses.
* @param signatureId Query node id.
* @param radius Maximum distance from @p signatureId (meters).
* @param optimizedPoses Pose graph after optimization (used for distances).
* @param maxGraphDepth Maximum graph distance (in terms of nodes) to bound the search.
* @return Map from node id to squared distance from @p signatureId.
*/
std::map<int, float> getNeighborsIdRadius(
int signatureId,
float radius,
const std::map<int, Transform> & optimizedPoses,
int maxGraphDepth) const;
/** @brief Marks @p locationId as intermediate (weight = -1); excludes it from loop closure. */
void convertToIntermediate(int locationId);
/**
* @brief Removes @p locationId from WM/STM and the database.
* @param locationId Id of the signature to delete.
* @param deletedWords Optional output: words whose reference count dropped to zero.
*/
void deleteLocation(int locationId, std::list<int> * deletedWords = 0);
/** @brief Forces @p locationId to be flushed to the database. */
void saveLocationData(int locationId);
/** @brief Removes any link between @p idA and @p idB (both directions). */
void removeLink(int idA, int idB);
/** @brief Strips raw images, scan, user data and/or occupancy grid from @p id to save memory (RAM). This doesn't clear any compressed data.*/
void removeRawData(int id, bool image = true, bool scan = true, bool userData = true, bool occupancyGrid = true);
/**
* @brief Merges @p id with a close neighbor (graph reduction).
* @param id Node to reduce.
* @param maxDistance Maximum distance to a neighbor to allow the merge (meters).
* @param keepLinkedInDb If true, the merged node is kept in the database (history only).
* @param direction Restrict merge target: 0=any, 1=previous neighbor, 2=next neighbor.
* @return Id of the node @p id was merged into, or 0 if no reduction was performed.
*/
int reduceNode(int id, float maxDistance = 0.0f, bool keepLinkedInDb = false, int direction = 0);
//getters
/** @return Working memory as { signature id, age } (does not include STM). */
const std::map<int, double> & getWorkingMem() const {return _workingMem;}
/** @return Set of signature ids currently in short-term memory. */
const std::set<int> & getStMem() const {return _stMem;}
/** @return Configured maximum STM size (@ref Parameters::kMemSTMSize()). */
int getMaxStMemSize() const {return _maxStMemSize;}
/** @brief Returns neighbor (sequential) links of @p signatureId; @p lookInDatabase also checks LTM. */
std::multimap<int, Link> getNeighborLinks(int signatureId,
bool lookInDatabase = false) const;
/** @brief Returns loop-closure links of @p signatureId; @p lookInDatabase also checks LTM. */
std::multimap<int, Link> getLoopClosureLinks(int signatureId,
bool lookInDatabase = false) const;
std::multimap<int, Link> getLinks(int signatureId, // can be also used to get links from landmarks
/**
* @brief Returns all links of @p signatureId (neighbor, loop, prior, gravity, ...).
* @param signatureId Source node id (can also be a landmark id).
* @param lookInDatabase Also query LTM for links.
* @param withLandmarks Include landmark links in the result.
*/
std::multimap<int, Link> getLinks(int signatureId,
bool lookInDatabase = false,
bool withLandmarks = false) const;
/** @brief Returns links of every signature; @p ignoreNullLinks drops empty placeholders. */
std::multimap<int, Link> getAllLinks(bool lookInDatabase, bool ignoreNullLinks = true, bool withLandmarks = false) const;
/** @return True if raw binary data (images, scans) is kept in memory after compression. */
bool isBinDataKept() const {return _binDataKept;}
/** @return Similarity threshold used by rehearsal (@ref Parameters::kMemRehearsalSimilarity()). */
float getSimilarityThreshold() const {return _similarityThreshold;}
/** @return Map from signature id to weight (rehearsal accumulation count) for WM and STM. */
std::map<int, int> getWeights() const;
/** @return Id of the most recently added signature, or 0 if none. */
int getLastSignatureId() const;
/**
* @brief Returns the most recent WM signature.
* @param ignoreIntermediateNodes If true, skips weight==-1 placeholder nodes.
*/
const Signature * getLastWorkingSignature(bool ignoreIntermediateNodes) const;
/** @brief Returns all nodes observing landmark @p landmarkId, mapped to the observation link. */
std::map<int, Link> getNodesObservingLandmark(int landmarkId, bool lookInDatabase) const;
/** @return Signature id labeled @p label, or 0 if not found. */
int getSignatureIdByLabel(const std::string & label, bool lookInDatabase = true) const;
/**
* @brief Assigns or removes a label on @p id.
* @param id Signature id; pass 0 to remove an existing label by name.
* @param label Label text; empty to remove.
* @return True if the label was applied.
*/
bool labelSignature(int id, const std::string & label);
/** @return Map from signature id to non-empty label (including STM+WM+LTM). */
const std::map<int, std::string> & getAllLabels() const {return _labels;}
/** @return Reverse landmark index: { landmark id (negative), nodes observing it }. */
const std::map<int, std::set<int> > & getLandmarksIndex() const {return _landmarksIndex;}
/** @return True if every persisted node (in LTM) is currently loaded in WM/STM. */
bool allNodesInWM() const {return _allNodesInWM;}
/**
* 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.
* @brief Attaches user data to signature @p id, compressing it on the fly if needed.
*
* The format is detected automatically: a single-row @c CV_8UC1 matrix is treated
* as already-compressed data and stored as-is; anything else is considered raw and
* compressed before being stored.
*
* @note If you pass one-dimensional unsigned 8-bit raw data, transpose it so it has
* multiple rows (not multiple columns), otherwise it will be misdetected as
* already compressed.
*
* @param id Target signature id (must be in WM/STM or LTM).
* @param data Raw or pre-compressed user data.
* @return True if the data was attached, false if @p id was not found.
*/
bool setUserData(int id, const cv::Mat & data);
int getDatabaseMemoryUsed() const; // in bytes
/** @return On-disk database size in bytes. */
int getDatabaseMemoryUsed() const;
/** @return Schema version of the open database (e.g. "0.20.0"). */
std::string getDatabaseVersion() const;
/** @return File path of the open database (empty if in-memory). */
std::string getDatabaseUrl() const;
/** @return Last @ref emptyTrash() flush time in seconds. */
double getDbSavingTime() const;
/** @return Map id of signature @p id; @p lookInDatabase also queries LTM. */
int getMapId(int id, bool lookInDatabase = false) const;
/** @return Odometry pose stored with @p signatureId (null if unknown). */
Transform getOdomPose(int signatureId, bool lookInDatabase = false) const;
/** @return Ground-truth pose stored with @p signatureId (null if unknown). */
Transform getGroundTruthPose(int signatureId, bool lookInDatabase = false) const;
const std::map<int, Transform> & getGroundTruths() const {return _groundTruths;} // only those in working+STM memory
/** @return Ground-truth poses for nodes currently in WM/STM. */
const std::map<int, Transform> & getGroundTruths() const {return _groundTruths;}
/**
* @brief Returns a GPS fix for @p id, falling back to the nearest GPS-tagged neighbor.
*
* Two cases:
* - If @p id has a GPS fix attached, @p gps is set to that fix and @p offsetENU
* is left at identity.
* - Otherwise, the graph is searched (via @ref getNeighborsId() with depth
* @p maxGraphDepth, ignoring loop closures) for the closest neighbor that has a
* GPS fix. When one is found, @p gps is set to that neighbor's fix and
* @p offsetENU is the rigid transform from that neighbor's pose to @p id,
* expressed in ENU coordinates (derived from the neighbor's heading/bearing).
* Applying @p offsetENU on top of the GPS-derived pose of the neighbor yields
* the ENU pose of @p id.
*
* If no GPS fix is found on @p id or any reachable neighbor, @p gps is returned
* empty (@c gps.stamp()==0) and @p offsetENU is identity.
*
* @param id Query signature id.
* @param gps Output GPS fix (empty if none found).
* @param offsetENU Output ENU-frame offset from the GPS-tagged node to @p id
* (identity when @p id itself carries the GPS fix or when none is found).
* @param lookInDatabase If true, also fetch missing nodes from LTM during the search.
* @param maxGraphDepth Maximum graph depth used to look for a GPS-tagged neighbor
* when @p id has none (0 = no depth limit, i.e. search the whole reachable graph).
*/
void getGPS(int id, GPS & gps, Transform & offsetENU, bool lookInDatabase, int maxGraphDepth = 0) const;
/**
* @brief Reads metadata of @p signatureId (no images/scan/words).
*
* @param signatureId Node id.
* @param odomPose Output odometry pose (null if not set).
* @param mapId Output map id.
* @param weight Output rehearsal weight.
* @param label Output label (empty if none).
* @param stamp Output timestamp (seconds, epoch).
* @param groundTruth Output ground-truth pose (null if not set).
* @param velocity Output 6-vector velocity (empty if not set).
* @param gps Output GPS fix (invalid if not set).
* @param sensors Output environmental sensor readings.
* @param lookInDatabase Also query LTM.
* @return True if @p signatureId was found.
*/
bool getNodeInfo(int signatureId,
Transform & odomPose,
int & mapId,
@@ -199,40 +495,183 @@ public:
GPS & gps,
EnvSensors & sensors,
bool lookInDatabase = false) const;
/** @return Compressed image blob for @p signatureId (empty if not stored). */
cv::Mat getImageCompressed(int signatureId) const;
/**
* @brief Loads sensor data of @p locationId from WM or LTM.
* @param images Include compressed RGB/depth images.
* @param scan Include laser scan blob.
* @param userData Include user data blob.
* @param occupancyGrid Include occupancy grid cells.
*/
SensorData getNodeData(int locationId, bool images, bool scan, bool userData, bool occupancyGrid) const;
/** @brief Loads the visual words, 3D points and global descriptors stored with @p nodeId. */
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;
/** @brief Loads mono and/or stereo camera calibration stored with @p nodeId. */
void getNodeCalibration(int nodeId,
std::vector<CameraModel> & models,
std::vector<StereoCameraModel> & stereoModels) const;
/**
* @brief Returns all signature ids in WM, STM and LTM.
* @param ignoreChildren If true, nodes not linked to graph anymore are excluded
*/
std::set<int> getAllSignatureIds(bool ignoreChildren = true) const;
/**
* @brief Reports whether the in-memory map has been modified since the database was last
* loaded or reset. @ref close() uses this flag to decide whether the database
* needs to be rewritten.
*
* The flag is cleared to @c false on construction and by @ref close() / @ref clear(),
* and is raised to @c true on any of the following events:
*
* - **@ref update() in mapping mode** (@ref Parameters::kMemIncrementalMemory() == @c true,
* see @ref isIncremental()): every successful call sets the flag, since a new
* @ref Signature is added to the graph and the visual word dictionary may grow.
* - **@ref update() in localization mode** (@ref isIncremental() == @c false): the flag
* is set only when @ref Parameters::kMemLocalizationDataSaved() is enabled
* (see @ref isLocalizationDataSaved()), i.e. when the new node must be persisted
* back to the database. Pure localization (the default,
* @ref Parameters::kMemLocalizationDataSaved() == @c false) leaves the flag at
* @c false even after many @ref update() calls, because nothing needs to be saved.
* - **@ref reduceNode()**: merging a node into a neighbor mutates the graph and
* marks the memory as changed (and also raises the link-changed flag).
* - **@ref init() dictionary repair**: when @ref init() rebuilds the visual word
* dictionary because words are missing from the database, the flag is forced to
* @c true so the regenerated dictionary is saved back on @ref close(), even if no
* new data was processed.
*
* Note that link-only modifications (e.g. @ref addLink(), @ref updateLink(),
* @ref removeLink()) update an independent @c _linksChanged flag, not this one.
*
* @return True if the memory has changed and would need to be persisted.
*/
bool memoryChanged() const {return _memoryChanged;}
/**
* @return True if the memory grows on @ref update() (mapping mode), false in localization mode.
* @see Parameters::kMemIncrementalMemory()
*/
bool isIncremental() const {return _incrementalMemory;}
/**
* @return True in localization mode when database writes are disabled
* (i.e. @ref isIncremental() is false and @ref Parameters::kMemLocalizationReadOnly() is enabled).
* @see Parameters::kMemIncrementalMemory()
* @see Parameters::kMemLocalizationReadOnly()
*/
bool isReadOnly() const {return !_incrementalMemory && _localizationReadOnly;}
/**
* @return True if data added during localization is persisted to the database.
* @see Parameters::kMemLocalizationDataSaved()
*/
bool isLocalizationDataSaved() const {return _localizationDataSaved;}
/** @return Signature with @p id in WM/STM, or null if not loaded. */
const Signature * getSignature(int id) const;
/** @return True if @p signatureId is in short-term memory. */
bool isInSTM(int signatureId) const {return _stMem.find(signatureId) != _stMem.end();}
/** @return True if @p signatureId is in working memory. */
bool isInWM(int signatureId) const {return _workingMem.find(signatureId) != _workingMem.end();}
/** @return True if @p signatureId is not in STM/WM, so when it is in LTM or non-existing. */
bool isInLTM(int signatureId) const {return !this->isInSTM(signatureId) && !this->isInWM(signatureId);}
/** @return True if signature ids are auto-generated, false if taken from sensor data id. */
bool isIDsGenerated() const {return _generateIds;}
/** @return Id of the last accepted global loop-closure node, or 0 if none. */
int getLastGlobalLoopClosureId() const {return _lastGlobalLoopClosureId;}
/** @return Feature extractor used to compute visual words. */
const Feature2D * getFeature2D() const {return _feature2D;}
/** @return True if graph reduction is enabled (@ref Parameters::kMemReduceGraph()). */
bool isGraphReduced() const {return _reduceGraph;}
/**
* @return Running per-axis maximum of the diagonal of every neighbor (odometry) link's
* information matrix observed so far, as a 6-vector
* (x, y, z, roll, pitch, yaw). Empty until at least one 6x6 neighbor link
* information matrix has been seen.
*
* This is a runtime statistic, not a configurable parameter: it is updated on
* @ref init() (over all loaded neighbor links) and on every @ref update() that
* adds a new neighbor link.
*
* It is consumed by @ref Rtabmap::getInformation() when
* @ref Parameters::kRGBDLoopCovLimited() is enabled, to clip loop-closure
* information matrices so a loop never claims higher confidence than odometry
* itself ever provided.
*
* @see Parameters::kRGBDLoopCovLimited()
*/
const std::vector<double> & getOdomMaxInf() const {return _odomMaxInf;}
/**
* @return True if the odometry pose orientation is used (instead of the IMU
* orientation) as the source of each new node's gravity link.
*
* When enabled, every new node gets a self-loop @ref Link::kGravity holding the
* rotation of the odometry pose passed to @ref update(). This assumes odometry is
* already gravity-aligned (e.g. a VIO front-end). When disabled, the gravity link
* is built from the IMU orientation in @ref SensorData::imu() if available.
*
* Gravity links are consumed by graph optimization only when
* @ref Parameters::kOptimizerGravitySigma() is non-zero.
*
* @see Parameters::kMemUseOdomGravity()
* @see Parameters::kOptimizerGravitySigma()
*/
bool isOdomGravityUsed() const {return _useOdometryGravity;}
/** @brief Writes a human-readable dump of WM/STM, links and weights to @p fileNameTree. */
void dumpMemoryTree(const char * fileNameTree) const;
/** @brief Dumps every internal map (signatures, words, dictionary) to text files in @p directory. */
virtual void dumpMemory(std::string directory) const;
/** @brief Dumps signatures' word ids (and 3D positions if @p words3D) to @p fileNameSign. */
virtual void dumpSignatures(const char * fileNameSign, bool words3D) const;
/** @brief Dumps the visual word dictionary: references to @p fileNameRef, descriptors to @p fileNameDesc. */
void dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const;
/** @return Approximate RAM usage of the in-memory state, in bytes. */
unsigned long getMemoryUsed() const; //Bytes
/**
* @brief Writes a Graphviz DOT file of the pose graph.
* @param fileName Output path.
* @param ids If non-empty, restrict the graph to these node ids.
*/
void generateGraph(const std::string & fileName, const std::set<int> & ids = std::set<int>());
/**
* @brief Removes spurious obstacle points from each node's local grid using a reference 2D map.
*
* For every node in @p poses, the node's local **obstacle** grid is loaded, each
* obstacle point is projected with @p poses into the reference @p map, and the
* point is kept only if either:
* - the reference @p map cell at its projection is not free space
* (i.e. the cell is an obstacle or unknown, value != 0), or
* - the reference @p map contains an obstacle cell (value == 100) within
* @p cropRadius cells of the projection.
*
* Points that fall on a free cell and have no obstacle neighbor within
* @p cropRadius are dropped. The filtered obstacle grid replaces the node's grid
* in WM/STM and (if the node is already persisted) in the database via
* @ref DBDriver::updateOccupancyGrid().
*
* **Ground and empty cells are not touched**: they are read and written back as-is.
*
* When @p filterScans is true, the same projection/filtering rule is also applied
* to each node's raw laser scan, and the rewritten scan is saved back to the
* database. This is useful to remove dynamic objects from the stored scans before
* re-meshing or re-exporting.
*
* @param poses Optimized poses used to project the local grids/scans into @p map.
* @param map Reference 2D occupancy grid (cell values: 0 free, 100 occupied,
* anything else unknown).
* @param xMin Reference map origin x in world coordinates (meters).
* @param yMin Reference map origin y in world coordinates (meters).
* @param cellSize Reference map resolution (meters/cell); must match the nodes' grid cell size.
* @param cropRadius Search radius (in cells) around each projected point used to
* accept points near an obstacle in @p map.
* @param filterScans If true, also filter and rewrite the raw laser scan attached
* to each node, using the same rule as for obstacle cells.
* @return Number of (node, grid or scan) modifications performed, or -1 on error
* (no database loaded, empty @p poses or empty @p map).
*/
int cleanupLocalGrids(
const std::map<int, Transform> & poses,
const cv::Mat & map,
@@ -242,10 +681,20 @@ public:
int cropRadius = 1,
bool filterScans = false);
//keypoint stuff
/** @return Visual word dictionary used for tf-idf likelihood and feature matching. */
const VWDictionary * getVWDictionary() const;
// RGB-D stuff
/**
* @brief Extracts a sub-graph (poses + links) for a set of node ids.
*
* Used by graph optimization callers to retrieve constraints for a region of interest.
*
* @param ids Ids to include.
* @param poses Output: odometry poses for @p ids.
* @param links Output: links between the nodes (and to landmarks if @p landmarksAdded).
* @param lookInDatabase If true, fetch missing data from LTM.
* @param landmarksAdded If true, also include landmark constraints.
*/
void getMetricConstraints(
const std::set<int> & ids,
std::map<int, Transform> & poses,
@@ -253,9 +702,31 @@ public:
bool lookInDatabase = false,
bool landmarksAdded = false);
/**
* @brief Computes the relative transform from @p fromS to @p toS using the registration pipeline.
* @param fromS Source signature (will be modified to cache extracted data).
* @param toS Target signature.
* @param guess Initial transform estimate (null if unknown).
* @param info Optional output with inlier counts, variance and diagnostics.
* @param useKnownCorrespondencesIfPossible If true, reuses existing word-id correspondences.
* @return The estimated transform, or a null @ref Transform on failure.
*/
Transform computeTransform(Signature & fromS, Signature & toS, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false) const;
/** @brief Convenience overload: loads signatures by id and forwards to the @ref Signature variant. */
Transform computeTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false);
/**
* @brief Refines a transform using ICP alignment of laser scans only.
* @return The refined transform, or a null @ref Transform on failure.
*/
Transform computeIcpTransform(const Signature & fromS, const Signature & toS, Transform guess, RegistrationInfo * info = 0) const;
/**
* @brief ICP registration of one node against an assembled cloud from multiple neighbors.
* @param newId New (query) node id.
* @param oldId Reference node id.
* @param poses Neighbor poses used to assemble the reference cloud.
* @param info Optional output with inlier counts and diagnostics.
* @return The estimated transform, or a null @ref Transform on failure.
*/
Transform computeIcpTransformMulti(
int newId,
int oldId,
@@ -296,6 +767,7 @@ private:
int getNi(int signatureId) const;
protected:
/** @brief Database driver owning the persistent storage (created by @ref init()). */
DBDriver * _dbDriver;
private:
corelib/include/rtabmap/core/Rtabmap.h
+702 -63
View File
@@ -52,22 +52,174 @@ class Signature;
class Optimizer;
class PythonInterface;
/**
* @class Rtabmap
* @brief Top-level RTAB-Map SLAM pipeline (mapping, localization and loop closure).
*
* Rtabmap orchestrates the full SLAM iteration. Each new sensor observation passed to
* @ref process() goes through the steps described below.
*
*
* @par 1. Memory update
*
* Done via @ref Memory::update(): a new @ref Signature is added to STM, the oldest
* STM entry is promoted to WM if STM is full, and rehearsal compares the new
* signature to the previous STM signature.
*
* When the robot barely moved since the previous frame (odometry displacement below
* @ref Parameters::kRGBDLinearUpdate() and @ref Parameters::kRGBDAngularUpdate()), the
* iteration is flagged as a "small displacement":
* - If a loop closure or localization was already accepted on a recent iteration,
* appearance-based global loop-closure detection and proximity detection by space
* are both skipped to avoid wasting work while the robot is stationary at an
* already-known location (only retrieval runs).
* - Otherwise (no recent loop closure / localization), both still run normally so
* a first-time loop closure can still be detected from a standstill.
*
* In either case, at the end of the iteration, if no loop closure, proximity
* detection or landmark observation latched onto the new node, it is deleted from
* @ref Memory so the map does not grow while the robot is idle.
*
* Rehearsal still runs first, so visually similar consecutive idle frames may also be
* merged into the previous STM signature (its weight is incremented and the new
* signature is discarded) when the similarity exceeds
* @ref Parameters::kMemRehearsalSimilarity().
*
*
* @par 2. Loop-closure hypothesis
*
* Scored via @ref Memory::computeLikelihood() and the recursive @ref BayesFilter
* (prior + observation update).
*
*
* @par 3. Hypothesis selection
*
* The highest posterior is compared against the loop-closure threshold
* (@ref Parameters::kRtabmapLoopThr()); if accepted, the loop-closure link is added
* and the pose graph is re-optimized by @ref Optimizer.
*
* In **RGB-D mode**, two extra checks must pass before the link is committed:
* - a valid geometric transform must be computed between the two candidate nodes by
* the registration pipeline (visual + optional ICP, see
* @ref Memory::computeTransform());
* - the resulting transform must not be rejected by the graph-optimization
* consistency check (see @ref Parameters::kRGBDOptimizeMaxError()).
*
* The optimization used by the consistency check depends on the operating mode:
* - In **mapping mode**, @ref optimizeCurrentMap() re-optimizes the local map
* around the current signature including the new link, then
* @ref graph::computeMaxGraphErrors() measures the worst per-link residual / its
* standard deviation. If the ratio exceeds @ref Parameters::kRGBDOptimizeMaxError(),
* the loop closure(s) added this iteration are removed from @ref Memory.
* - In **localization mode**, optimization is run on a sub-graph composed of the
* odometry cache (@ref Parameters::kRGBDMaxOdomCacheSize()), the newly added
* localization link and pose priors fixing the map nodes (weighted by
* @ref Parameters::kRGBDLocalizationPriorInf()). The same error-ratio check is
* applied; on failure the localization is rejected for this iteration but the
* persisted map and its links are left untouched.
*
* In both modes, if the same link is rejected twice in a row, @ref repairGraph() may
* also be attempted (within @ref Parameters::kRGBDOptimizeMaxErrorRepairRadius()) to
* drop the offending link instead of the new candidate.
*
* If either RGB-D check fails, the candidate is discarded and no link is added.
*
*
* @par 4. Retrieval
*
* Once a loop-closure hypothesis is selected, neighbors of the matched node are
* brought back from LTM into WM via @ref Memory::reactivateSignatures(), so the next
* iteration can compare against them too. Up to @ref Parameters::kRtabmapMaxRetrieved()
* nodes are pulled per iteration; nodes around the current path or local pose may
* also be retrieved (capped by @ref Parameters::kRtabmapMaxLocalRetrieved()).
*
* Retrieval (and the related node immunization) is only active when memory management
* is enabled, i.e. when @ref Parameters::kRtabmapTimeThr() or
* @ref Parameters::kRtabmapMemoryThr() is non-zero.
*
*
* @par 5. Proximity detection (RGB-D mode)
*
* Visual and scan-based local matches to nearby nodes, used in addition to the
* appearance-based loop closure.
*
* Candidate proximity links go through the same two RGB-D gates as loop closures
* above: a valid geometric transform must be computed by the registration pipeline,
* and the transform must not be rejected by the graph-optimization consistency check.
*
*
* @par 6. Transfer (WM to LTM)
*
* At the end of the iteration, if the iteration exceeded the configured time budget
* (@ref Parameters::kRtabmapTimeThr()) or WM exceeded its size budget
* (@ref Parameters::kRtabmapMemoryThr()), @ref Memory::forget() moves the oldest
* low-frequency signatures from WM to LTM (immunized nodes -- retrieved neighbors,
* the last localization node, etc. -- are kept in WM).
*
* Transfer is skipped when both thresholds are 0 (memory management disabled).
*
*
* @par 7. Map / localization output
*
* Optimized poses, current map correction and statistics are made available to
* callers via the getters below.
*
*
* @par Operating modes
*
* Selected by @ref Parameters::kMemIncrementalMemory() (see @ref Memory::isIncremental()):
* - **Mapping**: STM and WM grow; loop closures update the optimized graph.
* - **Localization**: STM/WM are frozen; the current node is matched against the
* persisted map and only @ref getLastLocalizationPose() is updated.
*
*
* @par Path planning
*
* Rtabmap also exposes basic graph-based path planning in RGB-D mode
* (@ref computePath(), @ref getPath(), @ref getPathStatus()), used by the GUI to
* navigate between mapped locations.
*
* When memory management is enabled (see step 4 Retrieval and step 6 Transfer), the
* retrieval step also pulls nodes along the currently planned path back from LTM into
* WM (capped by @ref Parameters::kRtabmapMaxLocalRetrieved()) so the robot is able to
* re-localize against upcoming waypoints as it follows the path, even when those
* nodes had been transferred out of WM earlier.
*
*
* @see Memory
* @see BayesFilter
* @see Optimizer
* @see Parameters
*/
class RTABMAP_CORE_EXPORT Rtabmap
{
public:
enum VhStrategy {kVhNone, kVhEpipolar, kVhUndef};
/** @brief Loop-closure verification strategy. */
enum VhStrategy {
kVhNone, ///< No verification: the highest hypothesis above threshold is accepted.
kVhEpipolar, ///< Epipolar geometry verification (mostly historical, RGB-only mode).
kVhUndef ///< Sentinel -- undefined.
};
public:
Rtabmap();
virtual ~Rtabmap();
/**
* @brief Main loop of rtabmap.
* @param data Sensor data to process.
* @param odomPose Odometry pose, should be non-null for RGB-D SLAM mode.
* @param covariance Odometry covariance.
* @param externalStats External statistics to be saved in the database for convenience
* @return true if data has been added to map.
* @brief Main RTAB-Map iteration: ingests one sensor frame and updates the map.
*
* Adds @p data to @ref Memory, runs the Bayes filter on the current likelihood,
* selects a loop-closure hypothesis if any, performs proximity detection,
* re-optimizes the graph as needed, and refreshes @ref getStatistics() and
* @ref getLastLocalizationPose().
*
* @param data Sensor data for this frame (images, scan, user data, ...).
* @param odomPose Odometry pose; must be non-null in RGB-D SLAM mode.
* Pass a null @ref Transform to fall back to appearance-only mode.
* @param odomCovariance 6x6 odometry covariance (default: identity).
* @param odomVelocity Optional 6-vector (vx, vy, vz, vroll, vpitch, vyaw).
* @param externalStats Extra named statistics to record in the database for this iteration.
* @return True if @p data was added to the map (i.e. an @ref update() succeeded).
*/
bool process(
const SensorData & data,
@@ -75,7 +227,13 @@ public:
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>());
// for convenience
/**
* @brief Convenience overload: builds a diagonal covariance from scalar variances.
*
* The 6x6 odometry covariance is constructed as
* @c diag(odomLinearVariance, odomLinearVariance, odomLinearVariance,
* odomAngularVariance, odomAngularVariance, odomAngularVariance).
*/
bool process(
const SensorData & data,
Transform odomPose,
@@ -83,113 +241,366 @@ public:
float odomAngularVariance,
const std::vector<float> & odomVelocity = std::vector<float>(),
const std::map<std::string, float> & externalStats = std::map<std::string, float>());
// for convenience, loop closure detection only
/**
* @brief Appearance-only convenience overload (loop-closure detection without odometry).
*
* Equivalent to processing @p image alone, with no odometry pose. Useful for offline
* loop-closure benchmarking on image sequences.
*
* @param image RGB or grayscale frame.
* @param id Optional frame id (0 = auto-generated).
* @param externalStats Extra named statistics to record in the database for this iteration.
*/
bool process(
const cv::Mat & image,
int id=0, const std::map<std::string, float> & externalStats = std::map<std::string, float>());
/**
* Initialize Rtabmap with parameters and a database
* @brief Initializes Rtabmap with parameters and a database.
*
* @param parameters Parameters overriding default parameters and database parameters
* (@see loadDatabaseParameters)
* @param 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.
* @param loadDatabaseParameters If an existing database is used (@see databasePath),
* the parameters inside are loaded and set to current
* Rtabmap instance.
* (see @p loadDatabaseParameters).
* @param databasePath Database input/output path. If empty, an in-memory database is
* used. If set and the file does not exist, it is created empty;
* if it exists, nodes and the visual word vocabulary are loaded
* into working memory.
* @param loadDatabaseParameters If true and an existing database is opened, the
* parameters stored inside the database are loaded and
* applied to this Rtabmap instance (then overridden by
* @p parameters).
*/
void init(const ParametersMap & parameters, const std::string & databasePath = "", bool loadDatabaseParameters = false);
/**
* Initialize Rtabmap with parameters from a configuration file and a database
* @param configFile Configuration file (*.ini) overriding default parameters and database parameters
* (@see loadDatabaseParameters)
* @param 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.
* @param loadDatabaseParameters If an existing database is used (@see databasePath),
* the parameters inside are loaded and set to current
* Rtabmap instance.
* @brief Initializes Rtabmap from a configuration file and a database.
*
* @param configFile Configuration file (*.ini) overriding default parameters and
* database parameters (see @p loadDatabaseParameters).
* @param databasePath Database input/output path; same semantics as the other @ref init().
* @param loadDatabaseParameters If true and an existing database is opened, the
* parameters stored inside the database are loaded and
* applied first, then overridden by values from @p configFile.
*/
void init(const std::string & configFile = "", const std::string & databasePath = "", bool loadDatabaseParameters = false);
/**
* Close rtabmap. This will delete rtabmap object if set.
* @param databaseSaved true=database saved, false=database discarded.
* @param databasePath output database file name, ignored if
* Db/Sqlite3InMemory=false (opened database is
* then overwritten).
* @brief Closes Rtabmap and releases the underlying @ref Memory.
*
* @param databaseSaved If true, the in-memory state is flushed to the database;
* if false, in-memory changes are discarded.
* @param ouputDatabasePath If non-empty, the database is copied to this path on
* close. If a database on disk was initially created/loaded on
* a different path, it will be updated with the latest changes
* and renamed to the output path.
*/
void close(bool databaseSaved = true, const std::string & ouputDatabasePath = "");
/** @return Working directory used for dumps, log files and temporary outputs. */
const std::string & getWorkingDir() const {return _wDir;}
/** @return True if RGB-D SLAM mode is enabled (@ref Parameters::kRGBDEnabled()). */
bool isRGBDMode() const { return _rgbdSlamMode; }
/** @return Id of the loop-closure hypothesis accepted at the last @ref process() iteration, or 0 if none. */
int getLoopClosureId() const {return _loopClosureHypothesis.first;}
/** @return Posterior probability of the accepted loop-closure hypothesis, or 0 if none. */
float getLoopClosureValue() const {return _loopClosureHypothesis.second;}
/** @return Id of the highest-posterior hypothesis at the last iteration (whether or not it was accepted). */
int getHighestHypothesisId() const {return _highestHypothesis.first;}
/** @return Posterior of the highest-posterior hypothesis at the last iteration. */
float getHighestHypothesisValue() const {return _highestHypothesis.second;}
/** @return Id of the last non-intermediate signature added to the map (0 if none). */
int getLastLocationId() const;
std::list<int> getWM() const; // working memory
std::set<int> getSTM() const; // short-term memory
int getWMSize() const; // working memory size
int getSTMSize() const; // short-term memory size
/** @return Working memory ids ordered as in @ref Memory::getWorkingMem(). */
std::list<int> getWM() const;
/** @return Short-term memory ids. */
std::set<int> getSTM() const;
/** @return Working memory size (number of WM signatures). */
int getWMSize() const;
/** @return Short-term memory size (number of STM signatures). */
int getSTMSize() const;
/** @return Per-signature weights (rehearsal counts) for WM and STM. */
std::map<int, int> getWeights() const;
/** @return Total number of signatures across WM, STM and LTM. */
int getTotalMemSize() const;
/** @return Wall-clock duration of the last @ref process() call, in seconds. */
double getLastProcessTime() const {return _lastProcessTime;};
/** @return True if @p locationId is currently in short-term memory. */
bool isInSTM(int locationId) const;
/** @return True if signature ids are auto-generated (vs. taken from @ref SensorData::id()). */
bool isIDsGenerated() const;
/** @return Statistics produced by the last @ref process() iteration. */
const Statistics & getStatistics() const;
/** @return Optimized poses of the current local map (last graph optimization result). */
const std::map<int, Transform> & getLocalOptimizedPoses() const {return _optimizedPoses;}
/** @return Constraints (links) of the current local map. */
const std::multimap<int, Link> & getLocalConstraints() const {return _constraints;}
/**
* @return Optimized pose of @p locationId in the current local map (identity if not present).
*/
Transform getPose(int locationId) const;
/**
* @return Transform mapping odometry frame to the optimized map frame.
*
* This is the correction applied to incoming odometry poses so they align with the
* latest graph optimization output. Updated whenever a loop closure or proximity
* detection re-optimizes the graph.
*
* In ROS terms, this corresponds to the standard @c /map -> @c /odom TF transform
* published by SLAM systems: composing it with the live odometry pose
* (@c /odom -> @c /base_link) yields the robot pose in the map frame.
*/
Transform getMapCorrection() const {return _mapCorrection;}
/** @return Owned @ref Memory (may be null before @ref init()). */
const Memory * getMemory() const {return _memory;}
/** @return Radius (meters) under which the current path goal is considered reached. */
float getGoalReachedRadius() const {return _goalReachedRadius;}
/** @return Local radius (meters) used by proximity detection and path planning queries. */
float getLocalRadius() const {return _localRadius;}
/**
* @return Last localized pose in the map frame.
*
* In **localization mode**, this is the corrected odometry pose of the last
* processed frame. In **mapping mode**, this is the last pose returned by
* @ref getLocalOptimizedPoses().
*/
const Transform & getLastLocalizationPose() const {return _lastLocalizationPose;}
float getTimeThreshold() const {return _maxTimeAllowed;} // in ms
void setTimeThreshold(float maxTimeAllowed); // in ms
int getMemoryThreshold() const {return _maxMemoryAllowed;} // in nodes
void setMemoryThreshold(int maxMemoryAllowed); // in nodes
/**
* @return Maximum allowed processing time per @ref process() call, in milliseconds.
* @see Parameters::kRtabmapTimeThr()
*/
float getTimeThreshold() const {return _maxTimeAllowed;}
/**
* @brief Sets the per-iteration time budget (ms).
*
* Drives how aggressively WM is transferred to LTM to keep iterations under the
* threshold. 0 disables the time bound.
*
* @note This setting and @ref setMemoryThreshold() are the only two switches that
* enable RTAB-Map's memory management (WM-to-LTM transfer, retrieval and node
* immunization). When both are 0, memory management is disabled and all
* signatures stay in working memory.
*
* @see Parameters::kRtabmapTimeThr()
*/
void setTimeThreshold(float maxTimeAllowed);
/**
* @return Maximum allowed WM size (number of signatures).
* @see Parameters::kRtabmapMemoryThr()
*/
int getMemoryThreshold() const {return _maxMemoryAllowed;}
/**
* @brief Sets the maximum number of signatures kept in WM (0 = unbounded).
*
* @note This setting and @ref setTimeThreshold() are the only two switches that
* enable RTAB-Map's memory management (WM-to-LTM transfer, retrieval and node
* immunization). When both are 0, memory management is disabled and all
* signatures stay in working memory.
*
* @see Parameters::kRtabmapMemoryThr()
*/
void setMemoryThreshold(int maxMemoryAllowed);
/**
* @brief Sets the localization prior pose used to seed the next @ref process() call
* (localization mode only).
*
* Tells RTAB-Map where the robot is currently located in the map frame, so that the
* very next call to @ref process() can align incoming odometry with the persisted
* map without waiting for a loop closure. Typical use cases: restoring localization
* after a session restart, applying an external pose estimate (e.g. from a GPS or
* a known starting point), or recovering from "kidnapped robot" situations.
*
* This call only stages state; it does **not** itself produce a non-identity
* @ref getMapCorrection(). The alignment between the odometry frame and the map
* frame is performed on the next @ref process() call, which consumes
* @p initialPose together with the incoming odometry pose. Two branches are taken
* depending on @ref Parameters::kRGBDOptimizeFromGraphEnd():
* - **false (default)**: @ref getMapCorrection() is set so that the live odometry
* pose is shifted to land on @p initialPose in the map frame (the optimized map
* is left untouched).
* - **true**: every optimized node pose is rigidly transformed so that the map
* itself moves to align with @p initialPose (the map correction stays close to
* identity).
*
* The transform applied is restricted by SLAM dimensionality: 3-DoF (x, y, yaw)
* for 2D SLAM, 4-DoF (x, y, z, yaw) when gravity is available
* (IMU orientation or @ref Memory::isOdomGravityUsed()) and
* @ref Parameters::kOptimizerGravitySigma() is non-zero, full 6-DoF otherwise.
*
* Side effects on the staged state:
* - @ref getLastLocalizationPose() is replaced by @p initialPose; the localization
* covariance, the last localization node id and the odometry cache used for
* loop-closure rejection are all cleared.
* - @ref getMapCorrection() is reset to identity and any backup is cleared.
* - If the current map has not been optimized yet (no entries in
* @ref getLocalOptimizedPoses()) and a last working signature exists, the map
* is optimized around that signature so the next @ref process() has something
* to localize against.
*
* After the next @ref process() consumes the prior, the nearest optimized node to
* @p initialPose is recorded as the last localization node.
*
* @param initialPose Robot pose in the map frame.
*
* @note No-op (with warning) in mapping mode.
*
* @see Parameters::kMemIncrementalMemory()
* @see Parameters::kRGBDOptimizeFromGraphEnd()
*/
void setInitialPose(const Transform & initialPose);
/**
* @brief Starts a new map session (next @ref process() will create a fresh map id).
*
* In **mapping mode**, this increments the map id, clears the local optimized graph
* and resets the Bayes filter.
* In **localization mode**, it resets the map correction, the localization node and
* the odometry cache; if @ref Parameters::kRtabmapRestartAtOrigin() is enabled, the
* last localization pose is reset to identity.
*
* @return The new map id (mapping mode), or -1 (localization mode).
*/
int triggerNewMap();
/**
* @brief Assigns or clears a label on signature @p id.
* @return True if the label was applied.
*/
bool labelLocation(int id, const std::string & label);
/**
* 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.
* @brief Attaches user data to signature @p id, compressing it on the fly if needed.
*
* The format is detected automatically: a single-row @c CV_8UC1 matrix is treated
* as already-compressed data and stored as-is; anything else is considered raw and
* compressed before being stored.
*
* @note If you pass one-dimensional unsigned 8-bit raw data, transpose it so it has
* multiple rows (not multiple columns), otherwise it will be misdetected as
* already compressed.
*
* @param id Target signature id (must be in WM/STM or LTM).
* @param data Raw or pre-compressed user data.
* @return True if the data was attached.
*/
bool setUserData(int id, const cv::Mat & data);
/**
* @brief Writes a Graphviz DOT file of the pose graph.
* @param path Output file path.
* @param id If non-zero, root the graph at @p id; otherwise use the last signature.
* @param margin Maximum graph depth around @p id to include.
*/
void generateDOTGraph(const std::string & path, int id=0, int margin=5);
/**
* @brief Exports the current pose graph to a text file.
*
* Forwards to @ref graph::exportPoses() after collecting either the optimized or
* the raw odometry poses (and the matching constraints when needed).
*
* @param path Output file path.
* @param optimized If true, export optimized poses; otherwise raw odometry poses.
* @param global If true, include nodes from LTM as well; otherwise only WM/STM.
* @param format Output format code; see @ref graph::exportPoses() for the full
* list of supported values (raw, RGBD-SLAM/TUM, KITTI, TORO, g2o, ...).
*
* @see graph::exportPoses()
*/
void exportPoses(
const std::string & path,
bool optimized,
bool global,
int format // 0=raw, 1=rgbd-slam format, 2=KITTI format, 3=TORO, 4=g2o
int format
);
/**
* @brief Clears all in-memory state and resets the database.
*
* In incremental mode, also clears the persisted map. In read-only memory mode,
* resets the in-memory state but leaves the database untouched.
*/
void resetMemory();
/** @brief Dumps the Bayes-filter prediction matrix to a file in the working directory. */
void dumpPrediction() const;
/** @brief Dumps the @ref Memory state (signatures, words, dictionary) to the working directory. */
void dumpData() const;
/**
* @brief Re-parses parameters and propagates them to owned sub-objects
* (@ref Memory, @ref BayesFilter, @ref Optimizer, ...).
*/
void parseParameters(const ParametersMap & parameters);
/** @return Current effective parameter map. */
const ParametersMap & getParameters() const {return _parameters;}
/**
* @brief Sets the working directory used for dumps, logs and temporary files.
*
* Can also be configured through @ref Parameters::kRtabmapWorkingDirectory() in the
* parameter map passed to @ref init() or @ref parseParameters().
*
* @see Parameters::kRtabmapWorkingDirectory()
*/
void setWorkingDirectory(std::string path);
/**
* @brief Removes the loop-closure link added at the last @ref process() iteration.
*
* Looks at the last non-intermediate signature in STM and erases any
* @ref Link::kGlobalClosure, @ref Link::kLocalSpaceClosure, @ref Link::kLocalTimeClosure
* or @ref Link::kUserClosure attached to it. The current optimized map is updated
* accordingly.
*/
void rejectLastLoopClosure();
/**
* @brief Deletes the most recent (non-intermediate) location from the map.
*
* Used by tools to undo the very last @ref process() iteration. In mapping mode,
* the optimized graph is recomputed without the deleted node.
*
* @note Locations whose neighbors include intermediate nodes are not supported.
*/
void deleteLastLocation();
/**
* @brief Replaces the current optimized poses and constraints with externally
* provided ones.
*
* Useful when graph optimization is performed outside of Rtabmap.
*
* @warning No consistency check is performed against the current @ref Memory state:
* @p poses and @p constraints overwrite the internal containers verbatim.
* The caller is responsible for ensuring that every id in @p poses (and
* every endpoint of every link in @p constraints) belongs to a signature
* currently in STM or WM (see @ref Memory::isInSTM() / @ref Memory::isInWM()).
* Passing poses for ids that are no longer loaded will leave dangling
* entries that may confuse subsequent @ref process() calls.
*/
void setOptimizedPoses(const std::map<int, Transform> & poses, const std::multimap<int, Link> & constraints);
/**
* @brief Returns a copy of signature @p id with optional payloads attached.
*
* Loads from WM/STM if present, otherwise from LTM. Selectively populates the
* returned @ref Signature with images, scan, user data, occupancy grid, visual
* words and global descriptors.
*/
Signature getSignatureCopy(int id, bool images, bool scan, bool userData, bool occupancyGrid, bool withWords, bool withGlobalDescriptors) const;
// Use getGraph() instead with withImages=true, withScan=true, withUserData=true and withGrid=true.
/**
* @brief Deprecated: use @ref getGraph() instead with @c withImages=true,
* @c withScan=true, @c withUserData=true and @c withGrid=true.
*/
RTABMAP_DEPRECATED
void get3DMap(std::map<int, Signature> & signatures,
std::map<int, Transform> & poses,
std::multimap<int, Link> & constraints,
bool optimized,
bool global) const;
/**
* @brief Extracts a full snapshot of the current pose graph.
*
* @param poses Output: pose for every selected node.
* @param constraints Output: links between selected nodes.
* @param optimized If true, return optimized poses; otherwise raw odometry poses.
* @param global If true, include nodes from LTM as well; otherwise only WM/STM.
* @param signatures Optional output: a copy of each node's @ref Signature (with the
* payloads requested by the @p with* flags).
* @param withImages Attach compressed RGB/depth images to @p signatures.
* @param withScan Attach laser scan blob.
* @param withUserData Attach user data blob.
* @param withGrid Attach occupancy grid cells.
* @param withWords Attach visual words (id, keypoints, 3D points, descriptors).
* @param withGlobalDescriptors Attach global descriptors.
*/
void getGraph(std::map<int, Transform> & poses,
std::multimap<int, Link> & constraints,
bool optimized,
@@ -201,8 +612,55 @@ public:
bool withGrid = false,
bool withWords = true,
bool withGlobalDescriptors = true) const;
std::map<int, Transform> getNodesInRadius(const Transform & pose, float radius, int k=0, std::map<int, float> * distsSqr=0); // If radius=0 and k=0, RGBD/LocalRadius is used. Can return landmarks.
std::map<int, Transform> getNodesInRadius(int nodeId, float radius, int k=0, std::map<int, float> * distsSqr=0); // If nodeId==0, return poses around latest node. If radius=0 and k=0, RGBD/LocalRadius is used. Can return landmarks and use landmark id (negative) as request.
/**
* @brief Returns optimized poses within a metric radius of @p pose.
*
* @param pose Query pose in the map frame.
* @param radius Search radius in meters (0 falls back to @ref Parameters::kRGBDLocalRadius()).
* @param k If non-zero, also cap the result to the @p k nearest neighbors.
* @param distsSqr Optional output: per-id squared distance to @p pose.
* @return Nodes (and possibly landmarks) within the radius, mapped to their pose.
* Landmarks have a negative id.
*/
std::map<int, Transform> getNodesInRadius(const Transform & pose, float radius, int k=0, std::map<int, float> * distsSqr=0);
/**
* @brief Returns optimized poses within a metric radius of node @p nodeId.
*
* @param nodeId Query node id. Pass 0 to query around the latest node. A negative
* id requests neighbors of the corresponding landmark.
* @param radius Search radius in meters (0 falls back to @ref Parameters::kRGBDLocalRadius()).
* @param k If non-zero, cap the result to the @p k nearest neighbors.
* @param distsSqr Optional output: per-id squared distance to @p nodeId.
* @return Nodes (and possibly landmarks) within the radius.
*/
std::map<int, Transform> getNodesInRadius(int nodeId, float radius, int k=0, std::map<int, float> * distsSqr=0);
/**
* @brief Post-processing: searches for additional loop closures over the existing graph.
*
* Clusters nearby optimized poses and runs registration between candidates that are
* not yet linked. New links are added to @ref Memory and the graph is re-optimized.
*
* @note The registration approach used here is the one configured in @ref Memory via
* @ref Parameters::kRegStrategy() (0=Vis, 1=Icp, 2=VisIcp), so the quality and
* sensor requirements of this pass mirror the live loop-closure pipeline.
*
* @note Candidate cluster pairs whose ids differ by less than
* @ref Parameters::kMemSTMSize(), or that are already reachable from each
* other within that many graph hops, are filtered out. This prevents trivial
* "loop closures" between temporally or topologically adjacent nodes.
*
* @param clusterRadiusMax Maximum metric distance (m) between two candidate nodes.
* @param clusterAngle Maximum angular distance (rad) between two candidate nodes.
* @param iterations Number of refinement passes.
* @param intraSession Include loop closures within the same map session.
* @param interSession Include loop closures between different map sessions.
* @param state Optional progress sink; cancellation requests are honored.
* @param clusterRadiusMin Minimum metric distance (m); pairs closer than this are
* considered already linked through neighbor links.
* @param toFromMapId If >=0, restrict candidate pairs to nodes belonging to this map id.
* @return Number of loop closures added, or -1 on error
* (e.g. not in RGB-D mode, no optimizer iterations).
*/
int detectMoreLoopClosures(
float clusterRadiusMax = 0.5f,
float clusterAngle = M_PI/6.0f,
@@ -212,11 +670,31 @@ public:
const ProgressState * state = 0,
float clusterRadiusMin = 0.0f,
int toFromMapId = -1);
/**
* @brief Runs a global bundle adjustment over the optimized graph.
*
* @param optimizerType Backend optimizer (e.g. 1=g2o); availability depends on
* what RTAB-Map was built with.
* @param rematchFeatures If true, re-match visual features between connected nodes
* before BA (otherwise reuse existing word-id correspondences).
* @param iterations Solver iterations (0 falls back to @ref Parameters::kOptimizerIterations()).
* @param pixelVariance Pixel reprojection variance used by the cost (0 falls back
* to @ref Parameters::kg2oPixelVariance()).
* @return True if BA was run and improved poses were stored.
*/
bool globalBundleAdjustment(
int optimizerType = 1 /*g2o*/,
bool rematchFeatures = true,
int iterations = 0,
float pixelVariance = 0.0f);
/**
* @brief Filters spurious obstacles from every node's local grid using a reference 2D map.
*
* Thin wrapper around @ref Memory::cleanupLocalGrids(); see that method for the
* exact filtering rule and the meaning of @p cropRadius and @p filterScans.
*
* @return Number of (node, grid or scan) modifications, or -1 on error.
*/
int cleanupLocalGrids(
const std::map<int, Transform> & mapPoses,
const cv::Mat & map,
@@ -225,28 +703,189 @@ public:
float cellSize,
int cropRadius = 1,
bool filterScans = false);
/**
* @brief Re-runs registration on every link of the current graph and updates the
* ones that converge.
*
* Useful after parameter changes to refresh stored transforms.
*
* @note The registration approach is the one configured in @ref Memory via
* @ref Parameters::kRegStrategy() (0=Vis, 1=Icp, 2=VisIcp). For each link,
* the link's existing relative transform (the constraint produced by the
* current optimized local graph) is passed as the initial guess to
* @ref Memory::computeTransform(), so links already close to convergence
* are refined locally rather than re-estimated from scratch.
*
* @return Number of links refined, or -1 if not in RGB-D mode.
*/
int refineLinks();
/**
* @brief Adds an external link to the map.
*
* The link's "from" and "to" endpoints must exist in memory (incremental mode) or
* in the optimized poses (localization mode). RGB-D mode only.
*
* @return True if the link was added.
*/
bool addLink(const Link & link);
/**
* @brief Converts an odometry covariance into an information matrix, clipping by
* @ref Memory::getOdomMaxInf() when @ref Parameters::kRGBDLoopCovLimited()
* is enabled.
*/
cv::Mat getInformation(const cv::Mat & covariance) const;
/**
* @brief Marks node ids whose data should be re-emitted on the next @ref process().
*
* The requested signatures are attached to the @ref Statistics object produced by
* the next @ref process() call (via the same mechanism as the regular "last signature
* data"), so consumers reading @ref getStatistics() pick them up alongside the
* normal output. Up to @ref Parameters::kRtabmapMaxRepublished() ids are emitted
* per iteration; any leftover ids stay queued for subsequent iterations until they
* are republished or fall out of the current graph.
*
* Pass an empty vector to clear the request set. Requires
* @ref Parameters::kRtabmapMaxRepublished() > 0 and
* @ref Parameters::kRtabmapPublishLastSignature() = true.
*/
void addNodesToRepublish(const std::vector<int> & ids);
int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success
void clearPath(int status); // -1=failed 0=idle/executing 1=success
/** @return Current path status: -1 = failed, 0 = idle / executing, 1 = success. */
int getPathStatus() const {return _pathStatus;}
/**
* @brief Clears the current path and sets its terminal status.
* @param status -1 = failed, 0 = idle / executing, 1 = success.
*/
void clearPath(int status);
/**
* @brief Plans a path from the current location to node @p targetNode.
*
* RGB-D mode only (requires @ref Parameters::kRGBDEnabled() = true).
*
* @param targetNode Destination node id (positive) or landmark id (negative).
* @param global If true, also search nodes in LTM; otherwise only the current
* optimized map.
* @return True if a path was computed; the result is available via @ref getPath().
*
* @see Parameters::kRGBDEnabled()
*/
bool computePath(int targetNode, bool global);
bool computePath(const Transform & targetPose, float tolerance = -1.0f); // only in current optimized map, tolerance (m) < 0 means RGBD/LocalRadius, 0 means infinite
/**
* @brief Plans a path in the current optimized map toward a metric goal pose.
*
* @param targetPose Goal pose in the map frame.
* @param tolerance Goal-acceptance tolerance (meters). A negative value falls back
* to @ref Parameters::kRGBDLocalRadius(); 0 means infinite tolerance.
* @return True if a path was computed.
*/
bool computePath(const Transform & targetPose, float tolerance = -1.0f);
/** @return The currently planned path as a sequence of (node id, pose) waypoints. */
const std::vector<std::pair<int, Transform> > & getPath() const {return _path;}
/** @return Upcoming waypoints (from the current path index onward). */
std::vector<std::pair<int, Transform> > getPathNextPoses() const;
/** @return Upcoming node ids (from the current path index onward). */
std::vector<int> getPathNextNodes() const;
/** @return Id of the current intermediate path goal (the node currently being chased). */
int getPathCurrentGoalId() const;
/** @return Index of the current waypoint in @ref getPath(). */
unsigned int getPathCurrentIndex() const {return _pathCurrentIndex;}
/** @return Index of the current intermediate goal in @ref getPath(). */
unsigned int getPathCurrentGoalIndex() const {return _pathGoalIndex;}
/** @return Transform from the final waypoint pose to the requested goal pose. */
const Transform & getPathTransformToGoal() const {return _pathTransformToGoal;}
/**
* @brief Returns optimized poses of WM nodes located in front of @p fromId.
*
* Candidates are first gathered around @p fromId, then STM nodes are excluded, the
* survivors are cropped to a forward-facing box of width @p radius (1 m behind,
* @p radius ahead, +/-@p radius laterally), and a KdTree radius search keeps the
* @p maxNearestNeighbors closest poses in that box.
*
* @note Mapping vs. localization mode differs only in how the initial candidate
* set is built:
* - In **mapping mode** (incremental), candidates are produced by a
* graph-radius walk from @p fromId: nodes reachable within @p maxDiffID
* graph hops AND within @p radius meters in the optimized poses.
* - In **localization mode**, the graph-hop restriction is ignored: every
* optimized pose within @p radius meters of @p fromId is considered.
* The forward-box crop and KdTree radius search that follow are identical
* in both modes.
*
* @param fromId Reference node (must be in @ref Memory and @ref getLocalOptimizedPoses()).
* @param maxNearestNeighbors Cap on the number of nodes returned.
* @param radius Maximum metric distance from @p fromId (meters).
* @param maxDiffID Maximum graph depth from @p fromId in mapping mode (0 = unlimited).
* Ignored in localization mode.
*/
std::map<int, Transform> getForwardWMPoses(int fromId, int maxNearestNeighbors, float radius, int maxDiffID) const;
/**
* @brief Segments a set of optimized poses into paths connected by neighbor links.
*
* Designed to be called on the result of a radius search around @p target (a set
* of @p poses already constrained to be metrically close to the goal). Within that
* radius, the method partitions the @p poses into one or more "paths" where each
* path is a connected component reachable from its starting node using **only
* neighbor (sequential) links** -- loop-closure links, landmark links and
* intermediate nodes are not used to traverse between members. Paths are produced
* one at a time, each starting from the still-unclaimed pose nearest to @p target;
* a candidate is added to the current path only if it has at least one neighbor
* link to a node already in the path.
*
* Used internally by proximity detection by space (see
* @ref Parameters::kRGBDProximityBySpace()) in two independent stages, each
* iterating over the segmented paths:
* - **One-to-one** (visual registration): runs registration between the current
* node and at most one node per neighbor-connected path, avoiding redundant
* attempts against nearby members of the same local trajectory.
* - **One-to-many** (scan matching, enabled when
* @ref Parameters::kRGBDProximityPathMaxNeighbors() > 0): on each path,
* neighboring nodes are assembled around the nearest pose on the path (up to
* the configured count, walked forward and backward) and their laser scans are
* merged for an ICP registration against the current scan. The
* neighbor-link-only structure of each path is what makes this assembly
* geometrically consistent.
*
* @param poses Candidate nodes with their optimized poses (typically pre-filtered
* to a radius around @p target).
* @param target Reference pose used to order paths: each path's starting node is
* the still-unclaimed pose closest to @p target.
* @param maxGraphDepth Maximum graph depth traversed from the starting node when
* gathering candidates for a path (0 = unlimited).
* @return Map from the starting node id of each path to its (node id -> pose) chain.
*/
std::map<int, std::map<int, Transform> > getPaths(const std::map<int, Transform> & poses, const Transform & target, int maxGraphDepth = 0) const;
/**
* @brief Applies the standard RTAB-Map likelihood adjustment.
*
* Normalizes raw likelihoods using mean and standard deviation across non-null
* values. Real-place entries with @c value <= @c mean + @c stdDev are clamped to
* @c 1.0; only entries above that threshold are scaled. The virtual place (the
* first key in @p likelihood, representing the "new place" hypothesis) is then
* set so that its likelihood reflects how peaked the real distribution is.
*
* The exact formulas are selected by @ref Parameters::kRtabmapVirtualPlaceLikelihoodRatio()
* (default 0, Angeli PhD formulation):
*
* - **Ratio = 0** (mean / std-dev formulation):
* - Real place above threshold: @c (value - (stdDev - epsilon)) / mean
* - Virtual place: @c mean / stdDev + 1 (when @c stdDev is non-trivial and a
* maximum exists; otherwise 2).
* The virtual place "wins" when the real-place distribution is flat
* (small @c stdDev relative to @c mean).
*
* - **Ratio != 0** (z-score formulation):
* - Real place above threshold: @c (value - mean) / stdDev (i.e. the z-score).
* - Virtual place: @c stdDev / (max - mean) + 1 (when @c max > @c mean;
* otherwise 2). The virtual place "wins" when no real candidate stands out
* far above the mean.
*
* In both formulations a low virtual-place likelihood favors a real-place loop
* closure on the next Bayes update; a high one favors the "new place" hypothesis.
*
* @see Parameters::kRtabmapVirtualPlaceLikelihoodRatio()
*/
void adjustLikelihood(std::map<int, float> & likelihood) const;
std::pair<int, float> selectHypothesis(const std::map<int, float> & posterior,
const std::map<int, float> & likelihood) const;
private:
void optimizeCurrentMap(int id,
@@ -289,8 +928,8 @@ private:
bool _publishRAMUsage;
bool _computeRMSE;
bool _saveWMState;
float _maxTimeAllowed; // in ms
unsigned int _maxMemoryAllowed; // signatures count in WM
float _maxTimeAllowed; ///< Per-iteration time budget (ms).
unsigned int _maxMemoryAllowed; ///< Maximum number of signatures kept in WM.
float _loopThr;
float _loopRatio;
float _aggressiveLoopThr;
@@ -331,7 +970,7 @@ private:
float _optimizationMaxErrorRepairRadius;
bool _startNewMapOnLoopClosure;
bool _startNewMapOnGoodSignature;
float _goalReachedRadius; // meters
float _goalReachedRadius; ///< Path-goal acceptance radius (meters).
bool _goalsSavedInUserData;
int _pathStuckIterations;
float _pathLinearVelocity;
@@ -377,16 +1016,16 @@ private:
std::map<int, Transform> _optimizedPoses;
std::multimap<int, Link> _constraints;
Transform _mapCorrection;
Transform _mapCorrectionBackup; // used in localization mode when odom is lost
Transform _lastLocalizationPose; // Corrected odometry pose. In mapping mode, this corresponds to last pose return by getLocalOptimizedPoses().
int _lastLocalizationNodeId; // for localization mode
Transform _mapCorrectionBackup; ///< Used in localization mode when odometry is lost.
Transform _lastLocalizationPose; ///< Corrected odometry pose; in mapping mode, last pose of getLocalOptimizedPoses().
int _lastLocalizationNodeId; ///< Last localization node id (localization mode).
cv::Mat _localizationCovariance;
std::map<int, std::pair<cv::Point3d, Transform> > _gpsGeocentricCache;
bool _currentSessionHasGPS;
LaserScan _globalScanMap;
std::map<int, Transform> _globalScanMapPoses;
std::map<int, Transform> _odomCachePoses; // used in localization mode to reject loop closures
std::multimap<int, Link> _odomCacheConstraints; // used in localization mode to reject loop closures
std::map<int, Transform> _odomCachePoses; ///< Odometry cache used to reject loop closures (localization mode).
std::multimap<int, Link> _odomCacheConstraints; ///< Odometry cache constraints (localization mode).
std::map<int, Transform> _markerPriors;
std::pair<int, int> _lastRejectedLoopClosureIds;