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/tests for Signature

Browse Source
This commit is contained in:
matlabbe
2025-12-23 17:33:08 -08:00
parent bc144d5172
commit 0b6a8a0828
3 changed files with 1287 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/Signature.h
+499 -28
View File
@@ -45,11 +45,66 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap
{
/**
* @class Signature
* @brief Represents a node in RTAB-Map's pose graph
*
* The Signature class is a fundamental building block of RTAB-Map's graph-based
* mapping system. Each signature represents a unique location or pose in the map
* and contains:
*
* - **Visual words**: Bag-of-words representation using visual features (keypoints,
* descriptors, 3D points) for place recognition
* - **Links**: Connections to other signatures (neighbors, loop closures, landmarks)
* - **Pose information**: Current pose, ground truth pose, velocity
* - **Sensor data**: Images, depth, laser scans, etc. captured at this location
* - **Metadata**: ID, map ID, timestamp, label, weight, modification flags
*
* Signatures are used for:
* - **Place recognition**: Comparing signatures to detect loop closures
* - **Graph construction**: Building the pose graph through links
* - **Localization**: Matching current observations to stored signatures
* - **Mapping**: Storing sensor data, visual features and local occupancy grid for each location
*
* The class tracks modification state to optimize database updates, and provides
* methods to manage visual words, links, landmarks, and pose information.
*
* @note Signatures are typically managed by the Memory class, which handles
* storage, retrieval, and graph operations.
*
* @see Link
* @see SensorData
* @see Memory
*/
class RTABMAP_CORE_EXPORT Signature
{
public:
/**
* @brief Default constructor
*
* Creates an empty signature with invalid ID (0), default map ID (-1),
* and all fields initialized to default values.
*/
Signature();
/**
* @brief Constructor with explicit parameters
*
* Creates a signature with the specified parameters. The sensor data ID
* will be automatically set to match the signature ID if not already set.
*
* @param id Unique signature ID (must be > 0 for valid signature)
* @param mapId Map ID this signature belongs to (default: -1 for no map)
* @param weight Weight/importance of this signature (default: 0)
* @param stamp Timestamp in seconds (default: 0.0)
* @param label Optional label/name for this signature (default: empty)
* @param pose Current pose of the signature (default: identity null transform)
* @param groundTruthPose Ground truth pose for evaluation (default: null transform)
* @param sensorData Sensor data captured at this location (default: empty)
*
* @note The signature is marked as modified and not saved by default.
*/
Signature(int id,
int mapId = -1,
int weight = 0,
@@ -58,67 +113,428 @@ public:
const Transform & pose = Transform(),
const Transform & groundTruthPose = Transform(),
const SensorData & sensorData = SensorData());
/**
* @brief Constructor from sensor data
*
* Creates a signature from sensor data. The signature ID is taken from
* the sensor data ID, and the ground truth pose is extracted from the
* sensor data if available.
*
* @param data Sensor data to create signature from
*
* @note The signature pose is initialized to identity transform.
*/
Signature(const SensorData & data);
/**
* @brief Virtual destructor
*/
virtual ~Signature();
/**
* Must return a value between >=0 and <=1 (1 means 100% similarity).
* @brief Compares this signature to another signature
*
* Computes a similarity score between this signature and another signature
* based on their visual words. The comparison uses bag-of-words matching
* to determine how similar the two locations are.
*
* @param signature The signature to compare against
* @return Similarity score between 0.0 and 1.0, where 1.0 means 100% similarity
*
* @note This method is used for loop closure detection and place recognition.
*/
float compareTo(const Signature & signature) const;
/**
* @brief Checks if this signature is considered "bad"
*
* A signature is considered bad if it has no valid visual words (all words
* are invalid or the signature has no words at all). Bad signatures cannot
* be used for place recognition.
*
* @return True if the signature has no valid visual words, false otherwise
*/
bool isBadSignature() const;
/**
* @brief Returns the signature ID
* @return The unique signature ID (0 if invalid/uninitialized)
*/
int id() const {return _id;}
/**
* @brief Returns the map ID
* @return The map ID this signature belongs to (-1 if no map assigned)
*/
int mapId() const {return _mapId;}
/**
* @brief Sets the weight/importance of this signature
*
* The weight represents the importance or priority of this signature,
* typically set by Rehearsal mechanism (see Memory::rehearsal).
* Higher weights may be used to prioritize certain signatures during
* memory management or loop closure detection.
*
* @param weight The weight value
*
* @note Automatically marks the signature as modified if the weight changes.
*/
void setWeight(int weight) {_modified=_weight!=weight;_weight = weight;}
/**
* @brief Returns the weight/importance of this signature
* @return The weight value
*/
int getWeight() const {return _weight;}
/**
* @brief Sets a label/name for this signature
*
* Labels are optional text identifiers that can be used to tag or
* categorize signatures (e.g., "room1", "corridor", "entrance").
*
* @param label The label string
*
* @note Automatically marks the signature as modified if the label changes.
*/
void setLabel(const std::string & label) {_modified=_label.compare(label)!=0;_label = label;}
/**
* @brief Returns the label/name of this signature
* @return The label string (empty if not set)
*/
const std::string & getLabel() const {return _label;}
/**
* @brief Returns the timestamp of this signature
* @return Timestamp in seconds (typically seconds since epoch)
*/
double getStamp() const {return _stamp;}
/**
* @brief Adds multiple links from a list
*
* Adds all links from the provided list to this signature's link collection.
*
* @param links List of links to add
*
* @note Each link must have `from()` equal to this signature's ID.
* @note Automatically marks links as modified.
* @see addLink()
*/
void addLinks(const std::list<Link> & links);
/**
* @brief Adds multiple links from a map
*
* Adds all links from the provided map to this signature's link collection.
*
* @param links Map of target IDs to links
*
* @note Each link must have `from()` equal to this signature's ID.
* @note The map's keys are the target IDs (`to()` values of the links).
* @note Automatically marks links as modified.
* @see addLink()
*/
void addLinks(const std::map<int, Link> & links);
/**
* @brief Adds a single link to this signature
*
* Adds a link connecting this signature to another signature. The link
* represents a constraint in the pose graph (e.g., neighbor, loop closure).
*
* @param link The link to add
*
* @note The link's `from()` must equal this signature's ID.
* @note The link's `to()` must be different from this signature's ID
* (except for self-referring links like pose priors or gravity).
* @note Duplicate links to the same target are not allowed (assertion failure).
* @note Automatically marks links as modified.
*/
void addLink(const Link & link);
/**
* @brief Checks if this signature has a link to a target signature
*
* @param idTo Target signature ID (0 to check for any link of the specified type)
* @param type Link type to check for (default: Link::kUndef to check any type)
* @return True if a matching link exists, false otherwise
*/
bool hasLink(int idTo, Link::Type type = Link::kUndef) const;
/**
* @brief Changes all link target IDs from one value to another
*
* Updates all links that point to `idFrom` to point to `idTo` instead.
* This is useful when merging or reorganizing the graph.
*
* @param idFrom Original target ID
* @param idTo New target ID
*
* @note Automatically marks links as modified.
*/
void changeLinkIds(int idFrom, int idTo);
/**
* @brief Removes all links from this signature
*
* @param keepSelfReferringLinks If true, preserves self-referring links
* (pose priors, gravity) that have from() == to()
*
* @note Automatically marks links as modified.
*/
void removeLinks(bool keepSelfReferringLinks = false);
/**
* @brief Removes a specific link to a target signature
*
* @param idTo Target signature ID of the link to remove
*
* @note Automatically marks links as modified.
*/
void removeLink(int idTo);
/**
* @brief Removes all virtual links from this signature
*
* Virtual links are links of type Link::kVirtualClosure.
*
* @note Automatically marks links as modified.
*/
void removeVirtualLinks();
/**
* @brief Adds a landmark observation to this signature
*
* Landmarks are persistent features in the environment that can be observed
* from multiple locations. This method adds a link representing the observation
* of a landmark from this signature's location.
*
* @param landmark Link representing the landmark observation
*
* @note Landmark IDs are typically negative to distinguish them from regular nodes.
*/
void addLandmark(const Link & landmark);
/**
* @brief Returns all landmark observations
* @return Const reference to the map of landmark IDs to landmark links
*/
const std::map<int, Link> & getLandmarks() const {return _landmarks;}
/**
* @brief Removes all landmark observations
*/
void removeLandmarks();
/**
* @brief Removes a specific landmark observation
* @param landmarkId The landmark ID to remove
*/
void removeLandmark(int landmarkId);
/**
* @brief Sets whether this signature has been saved to the database
* @param saved True if saved, false otherwise
*/
void setSaved(bool saved) {_saved = saved;}
/**
* @brief Sets the modification flag for this signature
*
* Marks the signature as modified (or unmodified). When set to true,
* also marks links as modified.
*
* @param modified True if modified, false otherwise
*/
void setModified(bool modified) {_modified = modified; _linksModified = modified;}
/**
* @brief Returns all links from this signature
* @return Const reference to the multimap of target IDs to links
*/
const std::multimap<int, Link> & getLinks() const {return _links;}
/**
* @brief Checks if this signature has been saved to the database
* @return True if saved, false otherwise
*/
bool isSaved() const {return _saved;}
/**
* @brief Checks if this signature has been modified
*
* Returns true if either the signature data or links have been modified.
*
* @return True if modified, false otherwise
*/
bool isModified() const {return _modified || _linksModified;}
/**
* @brief Checks if the links have been modified
* @return True if links are modified, false otherwise
*/
bool isLinksModified() const {return _linksModified;}
//visual words stuff
// Visual words management
/**
* @brief Removes all visual words from this signature
*
* Clears all visual words, keypoints, 3D points, and descriptors.
* The signature will be disabled after this operation.
*/
void removeAllWords();
/**
* @brief Changes visual word references from one word ID to another
*
* Updates all occurrences of `oldWordId` to `activeWordId` in the visual words.
* This is used when words are merged or reorganized in the dictionary.
*
* @param oldWordId The original word ID to replace
* @param activeWordId The new word ID to use
*
* @note Tracks word ID changes in `_wordsChanged` for reference.
*/
void changeWordsRef(int oldWordId, int activeWordId);
/**
* @brief Sets all visual words for this signature
*
* Sets the complete visual word representation including word IDs, keypoints,
* 3D points, and descriptors. All arrays must have matching sizes.
*
* @param words Multimap of word IDs to keypoint indices (allows duplicate words). The keypoint indices match the keypoints, points and descriptors.
* @param keypoints Vector of 2D keypoints in image coordinates. The keypoints must be in the same order as the words.
* @param words3 Vector of 3D points in base_link frame (with localTransform applied). The points must be in the same order as the words.
* @param descriptors Feature descriptors matrix (one row per word). The descriptors must be in the same order as the words.
*
* @note The signature is disabled after setting words (must be explicitly enabled).
* @note Invalid words (ID <= 0) are counted in `_invalidWordsCount`.
* @note All arrays must have the same size (number of words).
*/
void setWords(const std::multimap<int, int> & words, const std::vector<cv::KeyPoint> & keypoints, const std::vector<cv::Point3f> & words3, const cv::Mat & descriptors);
/**
* @brief Checks if this signature is enabled
*
* Enabled signatures can be used for place recognition and loop closure detection.
* More explicitly, it means that the signature is registered to the VWDictionary (see Memory::enableWordsRef).
*
* @return True if enabled, false otherwise
*/
bool isEnabled() const {return _enabled;}
/**
* @brief Sets whether this signature is enabled
* @param enabled True to enable, false to disable
*/
void setEnabled(bool enabled) {_enabled = enabled;}
/**
* @brief Returns the visual words map
*
* Returns a multimap of word IDs to keypoint indices. The multimap allows
* duplicate word IDs (a word can appear multiple times in the signature).
*
* @return Const reference to the words multimap
*/
const std::multimap<int, int> & getWords() const {return _words;}
/**
* @brief Returns the keypoints associated with visual words
* @return Const reference to the vector of 2D keypoints
*/
const std::vector<cv::KeyPoint> & getWordsKpts() const {return _wordsKpts;}
/**
* @brief Returns the count of invalid visual words
*
* Invalid words are those with ID <= 0. These words cannot be used
* for place recognition. However, they are still used for transform
* estimation after loop closures are detected with the valid words.
*
* @return Number of invalid words
*/
int getInvalidWordsCount() const {return _invalidWordsCount;}
/**
* @brief Returns the word ID change mapping
*
* Returns a map of old word IDs to new word IDs, tracking changes
* made through `changeWordsRef()`.
*
* @return Const reference to the word ID change map
*/
const std::map<int, int> & getWordsChanged() const {return _wordsChanged;}
/**
* @brief Returns the feature descriptors for visual words
* @return Const reference to the descriptors matrix (one row per word)
*/
const cv::Mat & getWordsDescriptors() const {return _wordsDescriptors;}
/**
* @brief Sets the feature descriptors for visual words
*
* Updates the descriptors matrix. The number of rows must match
* the number of visual words.
*
* @param descriptors Descriptors matrix (one row per word)
*/
void setWordsDescriptors(const cv::Mat & descriptors);
//metric stuff
// Pose and metric information
/**
* @brief Sets the pose of this signature
*
* The pose represents the position and orientation of this signature
* in the odometry coordinate frame. To get the pose in map coordinate frame,
* use the map correction transform computed by graph optimization and apply it to this pose.
*
* @param pose The transform representing the pose in the odometry coordinate frame.
*
* @code
* // Get pose in map coordinate frame
* Statistics stats;
* Transform poseInMapFrame = stats.mapCorrection() * signature.getPose();
* // poseInMapFrame is the pose in map coordinate frame
* // signature.getPose() returns the pose in odometry coordinate frame
* @endcode
*
* @see Statistics::mapCorrection()
* @see getPose()
*/
void setPose(const Transform & pose) {_pose = pose;}
/**
* @brief Sets the ground truth pose for evaluation
*
* Ground truth poses are used for evaluating localization accuracy
* and comparing against the estimated pose.
*
* @param pose The ground truth transform
*/
void setGroundTruthPose(const Transform & pose) {_groundTruthPose = pose;}
/**
* @brief Sets the velocity of this signature
*
* Sets the 6DOF velocity (linear and angular) at the time this signature
* was captured.
*
* @param vx Linear velocity in x direction (m/s)
* @param vy Linear velocity in y direction (m/s)
* @param vz Linear velocity in z direction (m/s)
* @param vroll Angular velocity around x axis (rad/s)
* @param vpitch Angular velocity around y axis (rad/s)
* @param vyaw Angular velocity around z axis (rad/s)
*/
void setVelocity(float vx, float vy, float vz, float vroll, float vpitch, float vyaw) {
_velocity = std::vector<float>(6,0);
_velocity[0]=vx;
@@ -129,45 +545,100 @@ public:
_velocity[5]=vyaw;
}
/**
* @brief Returns the 3D points of visual words
*
* Returns the 3D coordinates of visual words in the base_link frame
* (with localTransform applied).
*
* @return Const reference to the vector of 3D points
*/
const std::vector<cv::Point3f> & getWords3() const {return _words3;}
/**
* @brief Returns the pose of this signature
* @return Const reference to the pose transform in the odometry coordinate frame.
* @see setPose() for an example of how to get the pose in map coordinate frame.
*/
const Transform & getPose() const {return _pose;}
/**
* @brief Gets the pose covariance matrix
*
* Gets the covariance matrix representing the uncertainty
* between the current pose and the previous pose (e.g, from the odometry link with the previous signature).
*
* @return 6x6 covariance matrix (3x3 for translation, 3x3 for rotation)
*/
cv::Mat getPoseCovariance() const;
/**
* @brief Returns the ground truth pose
* @return Const reference to the ground truth transform
*/
const Transform & getGroundTruthPose() const {return _groundTruthPose;}
/**
* @brief Returns the velocity of this signature
* @return Const reference to the velocity vector [vx, vy, vz, vroll, vpitch, vyaw]
*/
const std::vector<float> & getVelocity() const {return _velocity;}
/**
* @brief Returns mutable access to the sensor data
* @return Reference to the sensor data
*/
SensorData & sensorData() {return _sensorData;}
/**
* @brief Returns const access to the sensor data
* @return Const reference to the sensor data
*/
const SensorData & sensorData() const {return _sensorData;}
unsigned long getMemoryUsed(bool withSensorData=true) const; // Return memory usage in Bytes
/**
* @brief Computes the memory usage of this signature
*
* Calculates the approximate memory footprint of this signature, including
* visual words, links, sensor data, and other internal structures.
*
* @param withSensorData If true, includes sensor data in the calculation
* @return Memory usage in bytes
*/
unsigned long getMemoryUsed(bool withSensorData=true) const;
private:
int _id;
int _mapId;
double _stamp;
std::multimap<int, Link> _links; // id, transform
std::map<int, Link> _landmarks;
int _weight;
std::string _label;
bool _saved; // If it's saved to bd
bool _modified;
bool _linksModified; // Optimization when updating signatures in database
int _id; ///< Unique signature ID (0 if invalid)
int _mapId; ///< Map ID this signature belongs to (-1 if no map)
double _stamp; ///< Timestamp in seconds
std::multimap<int, Link> _links; ///< Links to other signatures (target ID -> Link, allows multiple links per target)
std::map<int, Link> _landmarks; ///< Landmark observations (landmark ID -> Link)
int _weight; ///< Weight/importance of this signature
std::string _label; ///< Optional label/name for this signature
bool _saved; ///< Flag indicating if signature is saved to database
bool _modified; ///< Flag indicating if signature data has been modified
bool _linksModified; ///< Flag indicating if links have been modified (optimization for database updates)
// Contains all words (Some can be duplicates -> if a word appears 2
// times in the signature, it will be 2 times in this list)
// Words match with the CvSeq keypoints and descriptors
std::multimap<int, int> _words; // word <id, keypoint index>
std::vector<cv::KeyPoint> _wordsKpts;
std::vector<cv::Point3f> _words3; // in base_link frame (localTransform applied))
cv::Mat _wordsDescriptors;
std::map<int, int> _wordsChanged; // <oldId, newId>
bool _enabled;
int _invalidWordsCount;
/**
* @brief Visual words representation
*
* Contains all visual words for this signature. Words can be duplicates
* (a word can appear multiple times in the signature). Words match with
* the keypoints and descriptors arrays.
*/
std::multimap<int, int> _words; ///< Visual words: word ID -> keypoint index (multimap allows duplicates)
std::vector<cv::KeyPoint> _wordsKpts; ///< 2D keypoints in image coordinates
std::vector<cv::Point3f> _words3; ///< 3D points in base_link frame (with localTransform applied)
cv::Mat _wordsDescriptors; ///< Feature descriptors matrix (one row per word)
std::map<int, int> _wordsChanged; ///< Word ID change tracking: old ID -> new ID
bool _enabled; ///< Flag indicating if signature is enabled for place recognition
int _invalidWordsCount; ///< Count of invalid words (ID <= 0)
Transform _pose;
Transform _groundTruthPose;
std::vector<float> _velocity;
Transform _pose; ///< Current pose in map coordinate frame
Transform _groundTruthPose; ///< Ground truth pose for evaluation
std::vector<float> _velocity; ///< 6DOF velocity [vx, vy, vz, vroll, vpitch, vyaw]
SensorData _sensorData;
SensorData _sensorData; ///< Sensor data captured at this location (images, depth, scans, etc.)
};
} // namespace rtabmap
corelib/test/CMakeLists.txt
+6 -1
View File
@@ -89,4 +89,9 @@ gtest_discover_tests(test_stereocameramodel)
#Statistics.h
add_executable(test_statistics test_statistics.cpp)
target_link_libraries(test_statistics gtest_main rtabmap_core)
gtest_discover_tests(test_statistics)
gtest_discover_tests(test_statistics)
#Signature.h
add_executable(test_signature test_signature.cpp)
target_link_libraries(test_signature gtest_main rtabmap_core)
gtest_discover_tests(test_signature)
corelib/test/test_signature.cpp
+782
View File
@@ -0,0 +1,782 @@
#include <gtest/gtest.h>
#include <rtabmap/core/Signature.h>
#include <rtabmap/core/SensorData.h>
#include <rtabmap/core/Link.h>
#include <rtabmap/core/Transform.h>
#include <opencv2/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <cmath>
using namespace rtabmap;
// Constructor Tests
TEST(SignatureTest, DefaultConstructor)
{
Signature sig;
EXPECT_EQ(sig.id(), 0);
EXPECT_EQ(sig.mapId(), -1);
EXPECT_DOUBLE_EQ(sig.getStamp(), 0.0);
EXPECT_EQ(sig.getWeight(), 0);
EXPECT_TRUE(sig.getLabel().empty());
EXPECT_TRUE(sig.getPose().isNull());
EXPECT_TRUE(sig.getGroundTruthPose().isNull());
EXPECT_FALSE(sig.isSaved());
EXPECT_TRUE(sig.isModified());
EXPECT_TRUE(sig.isLinksModified());
EXPECT_FALSE(sig.isEnabled());
EXPECT_TRUE(sig.getWords().empty());
EXPECT_TRUE(sig.getWordsKpts().empty());
EXPECT_TRUE(sig.getWords3().empty());
EXPECT_TRUE(sig.getWordsDescriptors().empty());
EXPECT_TRUE(sig.getLinks().empty());
EXPECT_TRUE(sig.getLandmarks().empty());
EXPECT_TRUE(sig.isBadSignature());
}
TEST(SignatureTest, ParameterizedConstructor)
{
int id = 100;
int mapId = 1;
int weight = 5;
double stamp = 12345.678;
std::string label = "test_location";
Transform pose(1.0f, 2.0f, 3.0f, 0, 0, 0);
Transform groundTruth(1.1f, 2.1f, 3.1f, 0, 0, 0);
SensorData sensorData;
sensorData.setId(id);
Signature sig(id, mapId, weight, stamp, label, pose, groundTruth, sensorData);
EXPECT_EQ(sig.id(), id);
EXPECT_EQ(sig.mapId(), mapId);
EXPECT_EQ(sig.getWeight(), weight);
EXPECT_DOUBLE_EQ(sig.getStamp(), stamp);
EXPECT_EQ(sig.getLabel(), label);
EXPECT_FLOAT_EQ(sig.getPose().x(), 1.0f);
EXPECT_FLOAT_EQ(sig.getPose().y(), 2.0f);
EXPECT_FLOAT_EQ(sig.getPose().z(), 3.0f);
EXPECT_FLOAT_EQ(sig.getGroundTruthPose().x(), 1.1f);
EXPECT_FALSE(sig.isSaved());
EXPECT_TRUE(sig.isModified());
EXPECT_FALSE(sig.isEnabled());
}
TEST(SignatureTest, ParameterizedConstructorDefaults)
{
Signature sig(42);
EXPECT_EQ(sig.id(), 42);
EXPECT_EQ(sig.mapId(), -1);
EXPECT_EQ(sig.getWeight(), 0);
EXPECT_DOUBLE_EQ(sig.getStamp(), 0.0);
EXPECT_TRUE(sig.getLabel().empty());
EXPECT_TRUE(sig.getPose().isNull());
EXPECT_TRUE(sig.getGroundTruthPose().isNull());
}
TEST(SignatureTest, ConstructorFromSensorData)
{
SensorData sensorData;
sensorData.setId(200);
sensorData.setStamp(54321.0);
Transform gt(5.0f, 6.0f, 7.0f, 0, 0, 0);
sensorData.setGroundTruth(gt);
Signature sig(sensorData);
EXPECT_EQ(sig.id(), 200);
EXPECT_EQ(sig.mapId(), -1);
EXPECT_DOUBLE_EQ(sig.getStamp(), 54321.0);
EXPECT_TRUE(sig.getPose().isIdentity());
EXPECT_FLOAT_EQ(sig.getGroundTruthPose().x(), 5.0f);
EXPECT_FALSE(sig.isSaved());
EXPECT_TRUE(sig.isModified());
}
// Basic Getters/Setters Tests
TEST(SignatureTest, SetWeight)
{
Signature sig(1);
EXPECT_TRUE(sig.isModified());
sig.setModified(false);
sig.setWeight(10);
EXPECT_EQ(sig.getWeight(), 10);
EXPECT_TRUE(sig.isModified());
sig.setWeight(10); // Same value
EXPECT_FALSE(sig.isModified());
}
TEST(SignatureTest, SetLabel)
{
Signature sig(1);
EXPECT_TRUE(sig.isModified());
sig.setModified(false);
sig.setLabel("room1");
EXPECT_EQ(sig.getLabel(), "room1");
EXPECT_TRUE(sig.isModified());
sig.setLabel("room1"); // Same label
EXPECT_FALSE(sig.isModified());
sig.setLabel("room2");
EXPECT_EQ(sig.getLabel(), "room2");
EXPECT_TRUE(sig.isModified());
}
// Link Management Tests
TEST(SignatureTest, AddLink)
{
Signature sig(1);
Link link(1, 2, Link::kNeighbor, Transform::getIdentity());
sig.addLink(link);
const auto& links = sig.getLinks();
EXPECT_EQ(links.size(), 1u);
EXPECT_NE(links.find(2), links.end());
EXPECT_EQ(links.find(2)->second.type(), Link::kNeighbor);
EXPECT_TRUE(sig.isLinksModified());
}
TEST(SignatureTest, AddLinksFromList)
{
Signature sig(1);
std::list<Link> links;
links.push_back(Link(1, 2, Link::kNeighbor, Transform::getIdentity()));
links.push_back(Link(1, 3, Link::kNeighbor, Transform::getIdentity()));
links.push_back(Link(1, 4, Link::kGlobalClosure, Transform::getIdentity()));
sig.addLinks(links);
const auto& retrievedLinks = sig.getLinks();
EXPECT_EQ(retrievedLinks.size(), 3u);
EXPECT_NE(retrievedLinks.find(2), retrievedLinks.end());
EXPECT_NE(retrievedLinks.find(3), retrievedLinks.end());
EXPECT_NE(retrievedLinks.find(4), retrievedLinks.end());
EXPECT_TRUE(sig.isLinksModified());
}
TEST(SignatureTest, AddLinksFromMap)
{
Signature sig(1);
std::map<int, Link> links;
links[2] = Link(1, 2, Link::kNeighbor, Transform::getIdentity());
links[3] = Link(1, 3, Link::kNeighbor, Transform::getIdentity());
sig.addLinks(links);
const auto& retrievedLinks = sig.getLinks();
EXPECT_EQ(retrievedLinks.size(), 2u);
EXPECT_NE(retrievedLinks.find(2), retrievedLinks.end());
EXPECT_NE(retrievedLinks.find(3), retrievedLinks.end());
}
TEST(SignatureTest, HasLink)
{
Signature sig(1);
Link link1(1, 2, Link::kNeighbor, Transform::getIdentity());
Link link2(1, 3, Link::kGlobalClosure, Transform::getIdentity());
sig.addLink(link1);
sig.addLink(link2);
EXPECT_TRUE(sig.hasLink(2));
EXPECT_TRUE(sig.hasLink(3));
EXPECT_FALSE(sig.hasLink(4));
EXPECT_TRUE(sig.hasLink(2, Link::kNeighbor));
EXPECT_FALSE(sig.hasLink(2, Link::kGlobalClosure));
EXPECT_TRUE(sig.hasLink(3, Link::kGlobalClosure));
EXPECT_TRUE(sig.hasLink(0, Link::kNeighbor)); // Check for any neighbor link
EXPECT_TRUE(sig.hasLink(0, Link::kGlobalClosure)); // Check for any global closure
}
TEST(SignatureTest, ChangeLinkIds)
{
Signature sig(1);
Link link1(1, 10, Link::kNeighbor, Transform::getIdentity());
Link link2(1, 10, Link::kGlobalClosure, Transform::getIdentity());
sig.addLink(link1);
EXPECT_THROW(sig.addLink(link2), UException); // cannot have two links pointing on same signature
Link link3(1, 11, Link::kGlobalClosure, Transform::getIdentity());
sig.addLink(link3);
EXPECT_EQ(sig.getLinks().size(), 2u);
EXPECT_NE(sig.getLinks().find(11), sig.getLinks().end());
sig.changeLinkIds(11, 20);
const auto& links = sig.getLinks();
EXPECT_EQ(links.find(11), links.end());
EXPECT_NE(links.find(20), links.end());
EXPECT_EQ(links.count(20), 1u);
EXPECT_TRUE(sig.isLinksModified());
}
TEST(SignatureTest, RemoveLink)
{
Signature sig(1);
sig.addLink(Link(1, 2, Link::kNeighbor, Transform::getIdentity()));
sig.addLink(Link(1, 3, Link::kNeighbor, Transform::getIdentity()));
sig.addLink(Link(1, 4, Link::kGlobalClosure, Transform::getIdentity()));
EXPECT_EQ(sig.getLinks().size(), 3u);
sig.removeLink(3);
EXPECT_EQ(sig.getLinks().size(), 2u);
EXPECT_NE(sig.getLinks().find(2), sig.getLinks().end());
EXPECT_EQ(sig.getLinks().find(3), sig.getLinks().end());
EXPECT_NE(sig.getLinks().find(4), sig.getLinks().end());
EXPECT_TRUE(sig.isLinksModified());
}
TEST(SignatureTest, RemoveLinks)
{
Signature sig(1);
sig.addLink(Link(1, 2, Link::kNeighbor, Transform::getIdentity()));
sig.addLink(Link(1, 3, Link::kNeighbor, Transform::getIdentity()));
sig.addLink(Link(1, 1, Link::kPosePrior, Transform::getIdentity())); // Self-referring
EXPECT_EQ(sig.getLinks().size(), 3u);
sig.removeLinks(false); // Remove all, including self-referring
EXPECT_TRUE(sig.getLinks().empty());
// Add links again
sig.addLink(Link(1, 2, Link::kNeighbor, Transform::getIdentity()));
sig.addLink(Link(1, 1, Link::kPosePrior, Transform::getIdentity()));
sig.removeLinks(true); // Keep self-referring
EXPECT_EQ(sig.getLinks().size(), 1u);
EXPECT_NE(sig.getLinks().find(1), sig.getLinks().end());
}
TEST(SignatureTest, RemoveVirtualLinks)
{
Signature sig(1);
sig.addLink(Link(1, 2, Link::kNeighbor, Transform::getIdentity()));
sig.addLink(Link(1, 3, Link::kVirtualClosure, Transform::getIdentity()));
sig.addLink(Link(1, 4, Link::kGlobalClosure, Transform::getIdentity()));
EXPECT_EQ(sig.getLinks().size(), 3u);
sig.removeVirtualLinks();
EXPECT_EQ(sig.getLinks().size(), 2u);
EXPECT_NE(sig.getLinks().find(2), sig.getLinks().end());
EXPECT_EQ(sig.getLinks().find(3), sig.getLinks().end());
EXPECT_NE(sig.getLinks().find(4), sig.getLinks().end());
}
// Landmark Tests
TEST(SignatureTest, AddLandmark)
{
Signature sig(1);
Link landmark(1, -10, Link::kLandmark, Transform(1.0f, 0.0f, 0.0f, 0, 0, 0));
sig.addLandmark(landmark);
const auto& landmarks = sig.getLandmarks();
EXPECT_EQ(landmarks.size(), 1u);
EXPECT_NE(landmarks.find(-10), landmarks.end());
}
TEST(SignatureTest, RemoveLandmark)
{
Signature sig(1);
sig.addLandmark(Link(1, -10, Link::kLandmark, Transform::getIdentity()));
sig.addLandmark(Link(1, -20, Link::kLandmark, Transform::getIdentity()));
EXPECT_EQ(sig.getLandmarks().size(), 2u);
sig.removeLandmark(-10);
EXPECT_EQ(sig.getLandmarks().size(), 1u);
EXPECT_EQ(sig.getLandmarks().find(-10), sig.getLandmarks().end());
EXPECT_NE(sig.getLandmarks().find(-20), sig.getLandmarks().end());
}
TEST(SignatureTest, RemoveLandmarks)
{
Signature sig(1);
sig.addLandmark(Link(1, -10, Link::kLandmark, Transform::getIdentity()));
sig.addLandmark(Link(1, -20, Link::kLandmark, Transform::getIdentity()));
EXPECT_EQ(sig.getLandmarks().size(), 2u);
sig.removeLandmarks();
EXPECT_TRUE(sig.getLandmarks().empty());
}
// Modification Tracking Tests
TEST(SignatureTest, SetSaved)
{
Signature sig(1);
EXPECT_FALSE(sig.isSaved());
sig.setSaved(true);
EXPECT_TRUE(sig.isSaved());
sig.setSaved(false);
EXPECT_FALSE(sig.isSaved());
}
TEST(SignatureTest, SetModified)
{
Signature sig(1);
EXPECT_TRUE(sig.isModified());
EXPECT_TRUE(sig.isLinksModified());
sig.setModified(false);
EXPECT_FALSE(sig.isModified());
EXPECT_FALSE(sig.isLinksModified());
sig.setModified(true);
EXPECT_TRUE(sig.isModified());
EXPECT_TRUE(sig.isLinksModified());
}
// Visual Words Tests
TEST(SignatureTest, SetWords)
{
Signature sig(1);
std::multimap<int, int> words;
words.insert(std::make_pair(1, 0));
words.insert(std::make_pair(2, 1));
words.insert(std::make_pair(3, 2));
std::vector<cv::KeyPoint> keypoints;
keypoints.push_back(cv::KeyPoint(10.0f, 20.0f, 1.0f));
keypoints.push_back(cv::KeyPoint(30.0f, 40.0f, 1.0f));
keypoints.push_back(cv::KeyPoint(50.0f, 60.0f, 1.0f));
std::vector<cv::Point3f> words3;
words3.push_back(cv::Point3f(0.1f, 0.2f, 0.3f));
words3.push_back(cv::Point3f(0.4f, 0.5f, 0.6f));
words3.push_back(cv::Point3f(0.7f, 0.8f, 0.9f));
cv::Mat descriptors = cv::Mat::zeros(3, 32, CV_8UC1);
sig.setWords(words, keypoints, words3, descriptors);
EXPECT_EQ(sig.getWords().size(), 3u);
EXPECT_EQ(sig.getWordsKpts().size(), 3u);
EXPECT_EQ(sig.getWords3().size(), 3u);
EXPECT_EQ(sig.getWordsDescriptors().rows, 3);
EXPECT_FALSE(sig.isEnabled());
EXPECT_EQ(sig.getInvalidWordsCount(), 0);
EXPECT_FALSE(sig.isBadSignature());
}
TEST(SignatureTest, SetWordsWithInvalidWords)
{
Signature sig(1);
std::multimap<int, int> words;
words.insert(std::make_pair(1, 0));
words.insert(std::make_pair(0, 1)); // Invalid word (ID <= 0)
words.insert(std::make_pair(-1, 2)); // Invalid word (ID <= 0)
words.insert(std::make_pair(2, 3));
std::vector<cv::KeyPoint> keypoints(4);
std::vector<cv::Point3f> words3(4);
cv::Mat descriptors = cv::Mat::zeros(4, 32, CV_8UC1);
sig.setWords(words, keypoints, words3, descriptors);
EXPECT_EQ(sig.getWords().size(), 4u);
EXPECT_EQ(sig.getInvalidWordsCount(), 2);
EXPECT_FALSE(sig.isBadSignature()); // Has 2 valid words
}
TEST(SignatureTest, SetWordsAllInvalid)
{
Signature sig(1);
std::multimap<int, int> words;
words.insert(std::make_pair(0, 0));
words.insert(std::make_pair(-1, 1));
std::vector<cv::KeyPoint> keypoints(2);
std::vector<cv::Point3f> words3(2);
cv::Mat descriptors = cv::Mat::zeros(2, 32, CV_8UC1);
sig.setWords(words, keypoints, words3, descriptors);
EXPECT_EQ(sig.getInvalidWordsCount(), 2);
EXPECT_TRUE(sig.isBadSignature()); // No valid words
}
TEST(SignatureTest, SetWordsEmpty)
{
Signature sig(1);
std::multimap<int, int> words;
std::vector<cv::KeyPoint> keypoints;
std::vector<cv::Point3f> words3;
cv::Mat descriptors;
sig.setWords(words, keypoints, words3, descriptors);
EXPECT_TRUE(sig.getWords().empty());
EXPECT_TRUE(sig.isBadSignature());
}
TEST(SignatureTest, RemoveAllWords)
{
Signature sig(1);
std::multimap<int, int> words;
words.insert(std::make_pair(1, 0));
words.insert(std::make_pair(2, 1));
std::vector<cv::KeyPoint> keypoints(2);
std::vector<cv::Point3f> words3(2);
cv::Mat descriptors = cv::Mat::zeros(2, 32, CV_8UC1);
sig.setWords(words, keypoints, words3, descriptors);
EXPECT_EQ(sig.getWords().size(), 2u);
sig.removeAllWords();
EXPECT_TRUE(sig.getWords().empty());
EXPECT_TRUE(sig.getWordsKpts().empty());
EXPECT_TRUE(sig.getWords3().empty());
EXPECT_TRUE(sig.getWordsDescriptors().empty());
EXPECT_TRUE(sig.isBadSignature());
}
TEST(SignatureTest, ChangeWordsRef)
{
Signature sig(1);
std::multimap<int, int> words;
words.insert(std::make_pair(10, 0));
words.insert(std::make_pair(10, 1)); // Duplicate word ID
words.insert(std::make_pair(20, 2));
std::vector<cv::KeyPoint> keypoints(3);
std::vector<cv::Point3f> words3(3);
cv::Mat descriptors = cv::Mat::zeros(3, 32, CV_8UC1);
sig.setWords(words, keypoints, words3, descriptors);
EXPECT_EQ(sig.getWords().count(10), 2u);
EXPECT_EQ(sig.getWords().count(20), 1u);
sig.changeWordsRef(10, 30);
EXPECT_EQ(sig.getWords().count(10), 0u);
EXPECT_EQ(sig.getWords().count(30), 2u);
EXPECT_EQ(sig.getWords().count(20), 1u);
const auto& wordsChanged = sig.getWordsChanged();
EXPECT_NE(wordsChanged.find(10), wordsChanged.end());
EXPECT_EQ(wordsChanged.at(10), 30);
}
TEST(SignatureTest, ChangeWordsRefInvalid)
{
Signature sig(1);
std::multimap<int, int> words;
words.insert(std::make_pair(0, 0)); // Invalid word
words.insert(std::make_pair(1, 1));
std::vector<cv::KeyPoint> keypoints(2);
std::vector<cv::Point3f> words3(2);
cv::Mat descriptors = cv::Mat::zeros(2, 32, CV_8UC1);
sig.setWords(words, keypoints, words3, descriptors);
EXPECT_EQ(sig.getInvalidWordsCount(), 1);
sig.changeWordsRef(0, 10);
EXPECT_EQ(sig.getWords().count(0), 0u);
EXPECT_EQ(sig.getWords().count(10), 1u);
EXPECT_EQ(sig.getInvalidWordsCount(), 0);
}
TEST(SignatureTest, SetEnabled)
{
Signature sig(1);
EXPECT_FALSE(sig.isEnabled());
sig.setEnabled(true);
EXPECT_TRUE(sig.isEnabled());
sig.setEnabled(false);
EXPECT_FALSE(sig.isEnabled());
}
TEST(SignatureTest, SetWordsDisablesSignature)
{
Signature sig(1);
sig.setEnabled(true);
EXPECT_TRUE(sig.isEnabled());
std::multimap<int, int> words;
words.insert(std::make_pair(1, 0));
std::vector<cv::KeyPoint> keypoints(1);
std::vector<cv::Point3f> words3(1);
cv::Mat descriptors = cv::Mat::zeros(1, 32, CV_8UC1);
sig.setWords(words, keypoints, words3, descriptors);
EXPECT_FALSE(sig.isEnabled()); // Should be disabled after setWords
}
TEST(SignatureTest, SetWordsDescriptors)
{
Signature sig(1);
std::multimap<int, int> words;
words.insert(std::make_pair(1, 0));
words.insert(std::make_pair(2, 1));
std::vector<cv::KeyPoint> keypoints(2);
std::vector<cv::Point3f> words3(2);
cv::Mat descriptors = cv::Mat::zeros(2, 32, CV_8UC1);
sig.setWords(words, keypoints, words3, descriptors);
cv::Mat newDescriptors = cv::Mat::ones(2, 32, CV_8UC1) * 128;
sig.setWordsDescriptors(newDescriptors);
const auto& retrieved = sig.getWordsDescriptors();
EXPECT_EQ(retrieved.rows, 2);
EXPECT_EQ(retrieved.cols, 32);
EXPECT_EQ(retrieved.at<uchar>(0, 0), 128);
}
// Pose and Velocity Tests
TEST(SignatureTest, SetPose)
{
Signature sig(1);
Transform pose(1.0f, 2.0f, 3.0f, 0, 0, 0);
sig.setPose(pose);
const auto& retrieved = sig.getPose();
EXPECT_FLOAT_EQ(retrieved.x(), 1.0f);
EXPECT_FLOAT_EQ(retrieved.y(), 2.0f);
EXPECT_FLOAT_EQ(retrieved.z(), 3.0f);
}
TEST(SignatureTest, SetGroundTruthPose)
{
Signature sig(1);
Transform gt(10.0f, 20.0f, 30.0f, 0, 0, 0);
sig.setGroundTruthPose(gt);
const auto& retrieved = sig.getGroundTruthPose();
EXPECT_FLOAT_EQ(retrieved.x(), 10.0f);
EXPECT_FLOAT_EQ(retrieved.y(), 20.0f);
EXPECT_FLOAT_EQ(retrieved.z(), 30.0f);
}
TEST(SignatureTest, SetVelocity)
{
Signature sig(1);
sig.setVelocity(1.0f, 2.0f, 3.0f, 0.1f, 0.2f, 0.3f);
const auto& velocity = sig.getVelocity();
EXPECT_EQ(velocity.size(), 6u);
EXPECT_FLOAT_EQ(velocity[0], 1.0f); // vx
EXPECT_FLOAT_EQ(velocity[1], 2.0f); // vy
EXPECT_FLOAT_EQ(velocity[2], 3.0f); // vz
EXPECT_FLOAT_EQ(velocity[3], 0.1f); // vroll
EXPECT_FLOAT_EQ(velocity[4], 0.2f); // vpitch
EXPECT_FLOAT_EQ(velocity[5], 0.3f); // vyaw
}
TEST(SignatureTest, GetPoseCovariance)
{
Signature sig(1);
// Add a link to compute covariance
Link link(1, 2, Link::kNeighbor, Transform::getIdentity());
cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64FC1) * 10.0; // High information = low covariance
link.setInfMatrix(infMatrix);
sig.addLink(link);
cv::Mat covariance = sig.getPoseCovariance();
EXPECT_FALSE(covariance.empty());
EXPECT_EQ(covariance.rows, 6);
EXPECT_EQ(covariance.cols, 6);
}
// Sensor Data Tests
TEST(SignatureTest, SensorDataAccess)
{
Signature sig(1);
SensorData& mutableData = sig.sensorData();
mutableData.setId(1);
mutableData.setStamp(100.0);
const auto& constData = sig.sensorData();
EXPECT_EQ(constData.id(), 1);
EXPECT_DOUBLE_EQ(constData.stamp(), 100.0);
}
// Comparison Tests
TEST(SignatureTest, CompareTo)
{
Signature sig1(1);
Signature sig2(2);
// Empty signatures should have 0 similarity
float similarity = sig1.compareTo(sig2);
EXPECT_GE(similarity, 0.0f);
EXPECT_LE(similarity, 1.0f);
// Add some words to sig1
std::multimap<int, int> words1;
words1.insert(std::make_pair(1, 0));
words1.insert(std::make_pair(2, 1));
words1.insert(std::make_pair(3, 2));
std::vector<cv::KeyPoint> keypoints1(3);
std::vector<cv::Point3f> words3_1(3);
cv::Mat descriptors1 = cv::Mat::zeros(3, 32, CV_8UC1);
sig1.setWords(words1, keypoints1, words3_1, descriptors1);
// Add overlapping words to sig2
std::multimap<int, int> words2;
words2.insert(std::make_pair(1, 0));
words2.insert(std::make_pair(2, 1));
words2.insert(std::make_pair(4, 2)); // Different word
std::vector<cv::KeyPoint> keypoints2(3);
std::vector<cv::Point3f> words3_2(3);
cv::Mat descriptors2 = cv::Mat::zeros(3, 32, CV_8UC1);
sig2.setWords(words2, keypoints2, words3_2, descriptors2);
similarity = sig1.compareTo(sig2);
EXPECT_GE(similarity, 0.0f);
EXPECT_LE(similarity, 1.0f);
// Should have some similarity due to overlapping words (1 and 2)
EXPECT_GT(similarity, 0.0f);
}
TEST(SignatureTest, CompareToIdentical)
{
Signature sig1(1);
Signature sig2(2);
// Add identical words
std::multimap<int, int> words;
words.insert(std::make_pair(1, 0));
words.insert(std::make_pair(2, 1));
words.insert(std::make_pair(3, 2));
std::vector<cv::KeyPoint> keypoints(3);
std::vector<cv::Point3f> words3(3);
cv::Mat descriptors = cv::Mat::zeros(3, 32, CV_8UC1);
sig1.setWords(words, keypoints, words3, descriptors);
sig2.setWords(words, keypoints, words3, descriptors);
float similarity = sig1.compareTo(sig2);
EXPECT_GE(similarity, 0.0f);
EXPECT_LE(similarity, 1.0f);
// Identical words should have high similarity
EXPECT_GT(similarity, 0.5f);
}
// Memory Usage Tests
TEST(SignatureTest, GetMemoryUsed)
{
Signature sig(1);
unsigned long memory1 = sig.getMemoryUsed(false);
unsigned long memory2 = sig.getMemoryUsed(true);
EXPECT_GT(memory1, 0u);
EXPECT_GE(memory2, memory1); // With sensor data should be >= without
}
// Comprehensive Test
TEST(SignatureTest, ComprehensiveUsage)
{
Signature sig(100, 1, 10, 12345.678, "test_location");
// Set pose and velocity
sig.setPose(Transform(1.0f, 2.0f, 3.0f, 0, 0, 0));
sig.setGroundTruthPose(Transform(1.1f, 2.1f, 3.1f, 0, 0, 0));
sig.setVelocity(0.1f, 0.2f, 0.3f, 0.01f, 0.02f, 0.03f);
// Add links
sig.addLink(Link(100, 101, Link::kNeighbor, Transform::getIdentity()));
sig.addLink(Link(100, 102, Link::kGlobalClosure, Transform::getIdentity()));
// Add landmark
sig.addLandmark(Link(100, -10, Link::kLandmark, Transform(5.0f, 0.0f, 0.0f, 0, 0, 0)));
// Add words
std::multimap<int, int> words;
words.insert(std::make_pair(1, 0));
words.insert(std::make_pair(2, 1));
words.insert(std::make_pair(3, 2));
std::vector<cv::KeyPoint> keypoints(3);
std::vector<cv::Point3f> words3(3);
cv::Mat descriptors = cv::Mat::zeros(3, 32, CV_8UC1);
sig.setWords(words, keypoints, words3, descriptors);
sig.setEnabled(true);
// Verify everything
EXPECT_EQ(sig.id(), 100);
EXPECT_EQ(sig.getWeight(), 10);
EXPECT_EQ(sig.getLinks().size(), 2u);
EXPECT_EQ(sig.getLandmarks().size(), 1u);
EXPECT_EQ(sig.getWords().size(), 3u);
EXPECT_TRUE(sig.isEnabled());
EXPECT_FALSE(sig.isBadSignature());
EXPECT_FLOAT_EQ(sig.getPose().x(), 1.0f);
EXPECT_FLOAT_EQ(sig.getVelocity()[0], 0.1f);
}