Added Transform and VisualWord tests

corelib/include/rtabmap/core/Transform.h

@@ -38,28 +38,62 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+/**
+ * @class Transform
+ * @brief Represents a 3D rigid body transformation (rotation + translation).
+ *
+ * This class provides an abstraction over 3D transformations using a 3x4 matrix representation,
+ * with utilities for conversion to/from Eigen and OpenCV formats, interpolation, inversion,
+ * DoF reduction, and distance calculations. It is fundamental to pose estimation and motion handling
+ * within the RTAB-Map framework.
+ *
+ * The underlying data is stored in a `cv::Mat` (3x4, CV_32FC1), representing a rotation matrix and translation vector.
+ */
 class RTABMAP_CORE_EXPORT Transform
 {
 public:
 
-	// Zero by default
+	/**
+     * @brief Default constructor. Initializes to a null (all zeros) transform.
+     */
 	Transform();
-	// rotation matrix r## and origin o##
+	/**
+     * @brief Constructor from rotation matrix elements and translation components.
+     * @param r11...r33 Rotation matrix components.
+     * @param o14,o24,o34 Translation vector components.
+     */
 	Transform(float r11, float r12, float r13, float o14,
 		      float r21, float r22, float r23, float o24,
 			  float r31, float r32, float r33, float o34);
-	// should have 3 rows, 4 cols and type CV_32FC1
+	/**
+     * @brief Constructs a Transform from a 3x4 OpenCV matrix.
+     * @param transformationMatrix A 3x4 CV_32FC1 matrix.
+     */
 	Transform(const cv::Mat & transformationMatrix);
-	// x,y,z, roll,pitch,yaw
+	/**
+     * @brief Constructs a Transform from position and Euler angles (in radians).
+     * @param x, y, z     Translation components.
+     * @param roll, pitch, yaw Euler rotation angles (ZYX-convention).
+     */
 	Transform(float x, float y, float z, float roll, float pitch, float yaw);
-	// x,y,z, qx,qy,qz,qw
+	/**
+     * @brief Constructs a Transform from position and quaternion.
+     * @param x, y, z Translation components.
+     * @param qx, qy, qz, qw Quaternion rotation components.
+     */
 	Transform(float x, float y, float z, float qx, float qy, float qz, float qw);
-	// x,y, theta
+	/**
+     * @brief Constructs a 2D Transform (x, y, theta).
+     */
 	Transform(float x, float y, float theta);
 
+	/**
+     * @brief Returns a deep copy of the transform.
+     */
 	Transform clone() const;
 
-	float r11() const {return data()[0];}
+	// --- Accessors (rotation matrix elements, translation) ---
+	float r11() const {return data()[0];} //!< Rotation matrix element at row 1, col 1.
 	float r12() const {return data()[1];}
 	float r13() const {return data()[2];}
 	float r21() const {return data()[4];}
@@ -69,64 +103,157 @@ public:
 	float r32() const {return data()[9];}
 	float r33() const {return data()[10];}
 
-	float o14() const {return data()[3];}
+	float o14() const {return data()[3];} //!< Translation x
-	float o24() const {return data()[7];}
+	float o24() const {return data()[7];} //!< Translation y
-	float o34() const {return data()[11];}
+	float o34() const {return data()[11];} //!< Translation z
 
 	float & operator[](int index) {return data()[index];}
 	const float & operator[](int index) const {return data()[index];}
 	float & operator()(int row, int col) {return data()[row*4 + col];}
 	const float & operator()(int row, int col) const {return data()[row*4 + col];}
 
+	/**
+     * @brief Checks whether the transform is null (all zeros).
+     */
 	bool isNull() const;
+	/**
+     * @brief Checks whether the transform is identity.
+     */
 	bool isIdentity() const;
 
+	/**
+     * @brief Sets the transform to null (zero matrix).
+     */
 	void setNull();
+	/**
+     * @brief Sets the transform to the identity transform.
+     */
 	void setIdentity();
 
+	/**
+     * @brief Returns the internal OpenCV matrix (3x4).
+     */
 	const cv::Mat & dataMatrix() const {return data_;}
+	/**
+     * @brief Returns a pointer to the raw data (12 floats).
+     */
 	const float * data() const {return (const float *)data_.data;}
 	float * data() {return (float *)data_.data;}
+
+	/**
+     * @brief Returns the number of float elements (always 12).
+     */
 	int size() const {return 12;}
 
-	float & x() {return data()[3];}
+	/// Translation getters/setters
-	float & y() {return data()[7];}
+	float & x() {return data()[3];} //!< Translation x
-	float & z() {return data()[11];}
+	float & y() {return data()[7];} //!< Translation y
+	float & z() {return data()[11];} //!< Translation z
 	const float & x() const {return data()[3];}
 	const float & y() const {return data()[7];}
 	const float & z() const {return data()[11];}
 
+	/**
+     * @brief Returns 2D orientation (theta) in radians.
+     */
 	float theta() const;
 
+	/**
+     * @brief Returns whether the transform is invertible.
+     */
 	bool isInvertible() const;
+	/**
+     * @brief Returns the inverse of the transform.
+     */
 	Transform inverse() const;
+	/**
+     * @brief Returns only the rotation component.
+     */
 	Transform rotation() const;
+	/**
+     * @brief Returns only the translation component.
+     */
 	Transform translation() const;
+	/**
+     * @brief Converts to 3 DoF (x, y, theta).
+     */
 	Transform to3DoF() const;
+	/**
+     * @brief Converts to 4 DoF (x, y, z, yaw).
+     */
 	Transform to4DoF() const;
+	/**
+     * @brief Checks if the transform is 3 DoF (no pitch/roll).
+     */
 	bool is3DoF() const;
+	/**
+     * @brief Checks if the transform is 4 DoF (no pitch).
+     */
 	bool is4DoF() const;
 
+	/**
+     * @brief Returns the 3x3 rotation matrix (cv::Mat).
+     */
 	cv::Mat rotationMatrix() const;
+	/**
+     * @brief Returns the 3x1 translation matrix (cv::Mat).
+     */
 	cv::Mat translationMatrix() const;
 
+	/**
+     * @brief Extracts translation and Euler angles (radians).
+     */
 	void getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const;
+	/**
+     * @brief Extracts Euler angles (roll, pitch, yaw).
+     */
 	void getEulerAngles(float & roll, float & pitch, float & yaw) const;
+	/**
+     * @brief Extracts translation only.
+     */
 	void getTranslation(float & x, float & y, float & z) const;
+	/**
+     * @brief Returns angular difference (in radians) with another transform.
+     */
 	float getAngle(const Transform & t) const;
+	/**
+     * @brief Returns the Euclidean norm of the translation vector.
+     */
 	float getNorm() const;
+	/**
+     * @brief Returns the squared norm of the translation vector.
+     */
 	float getNormSquared() const;
+	/**
+     * @brief Returns the Euclidean distance to another transform.
+     */
 	float getDistance(const Transform & t) const;
+	/**
+     * @brief Returns the squared distance to another transform.
+     */
 	float getDistanceSquared(const Transform & t) const;
+	/**
+     * @brief Interpolates between this and another transform.
+     * @param t Interpolation factor [0, 1].
+     * @param other Target transform.
+     */
 	Transform interpolate(float t, const Transform & other) const;
+	/**
+     * @brief Normalizes the rotation matrix.
+     */
 	void normalizeRotation();
+	/**
+     * @brief Returns a string representation of the transform.
+     */
 	std::string prettyPrint() const;
 
+	/// Operator overloads
 	Transform operator*(const Transform & t) const;
 	Transform & operator*=(const Transform & t);
 	bool operator==(const Transform & t) const;
 	bool operator!=(const Transform & t) const;
 
+	// --- Eigen conversions ---
 	Eigen::Matrix4f toEigen4f() const;
 	Eigen::Matrix4d toEigen4d() const;
 	Eigen::Affine3f toEigen3f() const;
@@ -136,7 +263,14 @@ public:
 	Eigen::Quaterniond getQuaterniond() const;
 
 public:
+	// --- Static helpers ---
+
+    /**
+     * @brief Returns identity transform.
+     */
 	static Transform getIdentity();
+
+	/// Converts from Eigen representations
 	static Transform fromEigen4f(const Eigen::Matrix4f & matrix);
 	static Transform fromEigen4d(const Eigen::Matrix4d & matrix);
 	static Transform fromEigen3f(const Eigen::Affine3f & matrix);
@@ -146,40 +280,75 @@ public:
 	static Transform fromEigen3f(const Eigen::Matrix<float, 3, 4> & matrix);
 	static Transform fromEigen3d(const Eigen::Matrix<double, 3, 4> & matrix);
 
+	/**
+	 * @brief Returns the transform from RTAB-Map to OpenGL coordinate system.
+	 * @note Coordinate systems:
+	 * - OpenGL: x → right, y → up, z → out of the screen (toward viewer).
+	 * - RTAB-Map: x → forward, y → left, z → up.
+	 */
 	static Transform opengl_T_rtabmap() {return Transform(
 			 0.0f, -1.0f, 0.0f, 0.0f,
 			 0.0f,  0.0f, 1.0f, 0.0f,
 			-1.0f,  0.0f, 0.0f, 0.0f);}
+	/**
+	 * @brief Returns the transform from OpenGL to RTAB-Map coordinate system.
+	 * @note Coordinate systems:
+	 * - OpenGL: x → right, y → up, z → out of the screen (toward viewer).
+	 * - RTAB-Map: x → forward, y → left, z → up.
+	 */
 	static Transform rtabmap_T_opengl() {return Transform(
 			 0.0f,  0.0f,-1.0f, 0.0f,
 			-1.0f,  0.0f, 0.0f, 0.0f,
 			 0.0f,  1.0f, 0.0f, 0.0f);}
 
 	/**
-	 * Format (3 values): x y z
+     * @brief Parses a transform from a string representation.
-	 * Format (6 values): x y z roll pitch yaw
+     * Supported formats:
-	 * Format (7 values): x y z qx qy qz qw
+     * - (3 values) "x y z"
-	 * Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33
+     * - (6 values) "x y z roll pitch yaw" (ZYX-Euler convention)
-	 * Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz
+     * - (7 values) "x y z qx qy qz qw"
-	 */
+     * - (9 values, 3x3 rotation) "r11 r12 r13 r21 r22 r23 r31 r32 r33"
+	 * - (12 values, 3x4 rotation) "r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz"
+     */
 	static Transform fromString(const std::string & string);
+	/**
+     * @brief Checks if a string can be parsed into a transform.
+     */
 	static bool canParseString(const std::string & string);
 
+	/**
+     * @brief Retrieves the transform to a given timestamp.
+     * @param tfBuffer Buffer of timestamped transforms.
+     * @param stamp Requested timestamp.
+	 * @return transform at the requested timestamp, interpolated if it doesn't fall on a transform 
+	 *         with the exact same timestamp in the buffer. If the timestamp is older
+	 *         than the oldest timestamp or newer than the latest timestamp in the buffer, a 
+	 *         null transform is returned and a warning message is generated.
+     */
 	static Transform getTransform(
 				const std::map<double, Transform> & tfBuffer,
 				const double & stamp);
-	// Use Transform::getTransform() instead to get always accurate transforms.
+	/**
+     * @deprecated Use getTransform() instead.
+     */
 	RTABMAP_DEPRECATED static Transform getClosestTransform(
 				const std::map<double, Transform> & tfBuffer,
 				const double & stamp,
 				double * stampDiff);
 
 private:
-	cv::Mat data_;
+	cv::Mat data_; ///< 3x4 float matrix (rotation + translation)
 };
 
+/**
+ * @brief Output stream operator for Transform.
+ */
 RTABMAP_CORE_EXPORT std::ostream& operator<<(std::ostream& os, const Transform& s);
 
+/**
+ * @class TransformStamped
+ * @brief Associates a transform with a timestamp.
+ */
 class TransformStamped
 {
 public:

corelib/include/rtabmap/core/VisualWord.h

@@ -35,32 +35,95 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap
 {
 
+/**
+ * @class VisualWord
+ * @brief Represents a visual word (feature descriptor) used in the bag-of-words (BoW) model.
+ *
+ * A VisualWord holds a unique identifier, its descriptor (usually a feature vector from an image),
+ * and a record of where (in which signatures/images) it was observed. This is a key component in 
+ * loop closure detection and visual place recognition in RTAB-Map.
+ *
+ * The word keeps track of references (signature IDs) where it appears, and how many times it was observed 
+ * in each signature. It also tracks whether the word has been saved to a database.
+ */
 class RTABMAP_CORE_EXPORT VisualWord
 {
 public:
+	/**
+     * @brief Constructor.
+     * @param id          Unique ID of the visual word.
+     * @param descriptor  The descriptor associated with the word (e.g., ORB, SIFT).
+     * @param signatureId Optional signature ID where the word is first observed.
+     */
 	VisualWord(int id, const cv::Mat & descriptor, int signatureId = 0);
+
+	/**
+     * @brief Destructor.
+     */
 	~VisualWord();
 
+	/**
+     * @brief Adds a reference to this word for the given signature ID.
+     * @param signatureId ID of the signature in which this word is observed.
+     */
 	void addRef(int signatureId);
+
+	/**
+     * @brief Removes all references to this word for the given signature ID.
+     * @param signatureId ID of the signature from which references will be removed.
+     * @return Number of references removed.
+     */
 	int removeAllRef(int signatureId);
+
+	/**
+     * @brief Estimates the memory used by this visual word in bytes.
+     * @return Size in bytes.
+     */
 	unsigned long getMemoryUsed() const;
 
+	/**
+     * @brief Returns the total number of references (across all signatures).
+     * @return Number of total references.
+     */
 	int getTotalReferences() const {return _totalReferences;}
-	int id() const {return _id;}
-	const cv::Mat & getDescriptor() const {return _descriptor;}
-	const std::map<int, int> & getReferences() const {return _references;} // (signature id , occurrence in the signature)
 
+	/**
+     * @brief Returns the ID of this visual word.
+     * @return Word ID.
+     */
+	int id() const {return _id;}
+
+	/**
+     * @brief Returns the feature descriptor of this word.
+     * @return The OpenCV matrix descriptor.
+     */
+	const cv::Mat & getDescriptor() const {return _descriptor;}
+
+	/**
+     * @brief Returns the references of this word.
+     * @return A map of (signature ID, occurrence count).
+     */
+	const std::map<int, int> & getReferences() const {return _references;}
+
+	/**
+     * @brief Checks if the word has been saved to the database.
+     * @return True if saved, false otherwise.
+     */
 	bool isSaved() const {return _saved;}
+
+	/**
+     * @brief Sets the saved flag for the word.
+     * @param saved True if the word has been saved to the database.
+     */
 	void setSaved(bool saved) {_saved = saved;}
 
 private:
-	int _id;
+	int _id;                                  ///< Unique ID of the visual word.
-	cv::Mat _descriptor;
+	cv::Mat _descriptor;                      ///< Feature descriptor (e.g., ORB, SIFT).
-	bool _saved; // If it's saved to db
+	bool _saved;                              ///< Whether the word is saved to the database.
 
-	int _totalReferences;
+	int _totalReferences;                     ///< Total reference count across all signatures.
-	std::map<int, int> _references; // (signature id , occurrence in the signature)
+	std::map<int, int> _references;           ///< Active references: map of (signature ID, occurrence count).
-	std::map<int, int> _oldReferences; // (signature id , occurrence in the signature)
 };
 
 } // namespace rtabmap

corelib/src/Transform.cpp

@@ -530,35 +530,35 @@ Transform Transform::getTransform(
 				const double & stamp)
 {
 	UASSERT(!tfBuffer.empty());
-	std::map<double, Transform>::const_iterator imuIterB = tfBuffer.lower_bound(stamp);
+	std::map<double, Transform>::const_iterator iterB = tfBuffer.lower_bound(stamp);
-	std::map<double, Transform>::const_iterator imuIterA = imuIterB;
+	std::map<double, Transform>::const_iterator iterA = iterB;
-	if(imuIterA != tfBuffer.begin())
+	if(iterA != tfBuffer.begin())
 	{
-		imuIterA = --imuIterA;
+		iterA = --iterA;
 	}
-	if(imuIterB == tfBuffer.end())
+	if(iterB == tfBuffer.end())
 	{
-		imuIterB = --imuIterB;
+		iterB = --iterB;
 	}
-	Transform imuT;
+	Transform t;
-	if(imuIterB->first == stamp)
+	if(iterB->first == stamp)
 	{
-		imuT = imuIterB->second;
+		t = iterB->second;
 	}
-	else if(imuIterA != imuIterB)
+	else if(iterA != iterB)
 	{
 		//interpolate:
-		imuT = imuIterA->second.interpolate((stamp-imuIterA->first) / (imuIterB->first-imuIterA->first), imuIterB->second);
+		t = iterA->second.interpolate((stamp-iterA->first) / (iterB->first-iterA->first), iterB->second);
 	}
-	else if(stamp > imuIterB->first)
+	else if(stamp > iterB->first)
 	{
-		UWARN("No transform found for stamp %f! Latest is %f", stamp, imuIterB->first);
+		UWARN("No transform found for stamp %f! Latest is %f", stamp, iterB->first);
 	}
 	else
 	{
-		UWARN("No transform found for stamp %f! Earliest is %f", stamp, imuIterA->first);
+		UWARN("No transform found for stamp %f! Earliest is %f", stamp, iterA->first);
 	}
-	return imuT;
+	return t;
 }
 
 Transform Transform::getClosestTransform(

corelib/src/VisualWord.cpp

@@ -73,7 +73,6 @@ unsigned long VisualWord::getMemoryUsed() const
 {
 	unsigned long memoryUsage = sizeof(VisualWord);
 	memoryUsage += _references.size() * (sizeof(int)*2+sizeof(std::map<int ,int>::iterator)) + sizeof(std::map<int ,int>);
-	memoryUsage += _oldReferences.size() * (sizeof(int)*2+sizeof(std::map<int ,int>::iterator)) + sizeof(std::map<int ,int>);
 	memoryUsage += _descriptor.total() * _descriptor.elemSize();
 	return memoryUsage;
 }

corelib/test/CMakeLists.txt

@@ -50,3 +50,13 @@ gtest_discover_tests(test_util3d_motion_estimation)
 add_executable(test_util3d_surface test_util3d_surface.cpp)
 target_link_libraries(test_util3d_surface gtest_main rtabmap_core)
 gtest_discover_tests(test_util3d_surface)
+
+#VisualWord.h
+add_executable(VisualWordTests VisualWordTests.cpp)
+target_link_libraries(VisualWordTests gtest_main rtabmap_core)
+gtest_discover_tests(VisualWordTests)
+
+#Transform.h
+add_executable(TransformTests TransformTests.cpp)
+target_link_libraries(TransformTests gtest_main rtabmap_core)
+gtest_discover_tests(TransformTests)

corelib/test/TransformTests.cpp

@@ -0,0 +1,220 @@
+#include <gtest/gtest.h>
+#include <rtabmap/core/Transform.h>
+#include <cmath>
+
+using namespace rtabmap;
+
+// Helper for floating point comparison
+static bool approxEqual(float a, float b, float epsilon = 1e-5f)
+{
+    return std::fabs(a - b) < epsilon;
+}
+
+// ---------- TEST CASES ----------
+
+TEST(TransformTests, DefaultConstructor)
+{
+    Transform t;
+    EXPECT_TRUE(t.isNull());
+    EXPECT_FALSE(t.isIdentity());
+}
+
+TEST(TransformTests, Identity)
+{
+    Transform t = Transform::getIdentity();
+    EXPECT_TRUE(t.isIdentity());
+    EXPECT_FALSE(t.isNull());
+
+    // Identity * Identity = Identity
+    Transform result = t * Transform::getIdentity();
+    EXPECT_TRUE(result.isIdentity());
+}
+
+TEST(TransformTests, EqualityOperators)
+{
+    Transform t1 = Transform::getIdentity();
+    Transform t2 = Transform::getIdentity();
+
+    EXPECT_TRUE(t1 == t2);
+    EXPECT_FALSE(t1 != t2);
+
+    t2.x() = 1.0f;
+    EXPECT_FALSE(t1 == t2);
+    EXPECT_TRUE(t1 != t2);
+}
+
+TEST(TransformTests, TranslationConstructor)
+{
+    Transform t(1.0f, 2.0f, 3.0f, 0, 0, 0);
+    EXPECT_TRUE(approxEqual(t.x(), 1.0f));
+    EXPECT_TRUE(approxEqual(t.y(), 2.0f));
+    EXPECT_TRUE(approxEqual(t.z(), 3.0f));
+}
+
+TEST(TransformTests, MatrixConstructor)
+{
+    cv::Mat mat = (cv::Mat_<float>(3, 4) <<
+        1, 0, 0, 1,
+        0, 1, 0, 2,
+        0, 0, 1, 3);
+    Transform t(mat);
+    EXPECT_TRUE(approxEqual(t.x(), 1.0f));
+    EXPECT_TRUE(approxEqual(t.y(), 2.0f));
+    EXPECT_TRUE(approxEqual(t.z(), 3.0f));
+}
+
+TEST(TransformTests, Inverse)
+{
+    Transform t(1, 0, 0, 1,
+                0, 1, 0, 2,
+                0, 0, 1, 3);
+    Transform inv = t.inverse();
+    Transform id = t * inv;
+    EXPECT_TRUE(id.isIdentity());
+}
+
+TEST(TransformTests, Interpolation)
+{
+    Transform t1(0, 0, 0, 0, 0, 0);
+    Transform t2(0, 0, 1, 0, 0, 0);
+    Transform mid = t1.interpolate(0.5f, t2);
+    EXPECT_TRUE(approxEqual(mid.z(), 0.5f));
+}
+
+TEST(TransformTests, NormAndDistance)
+{
+    Transform t1(0, 0, 0, 0, 0, 0);
+    Transform t2(1, 2, 2, 0, 0, 0);
+
+    EXPECT_TRUE(approxEqual(t2.getNorm(), 3.0f));
+    EXPECT_TRUE(approxEqual(t1.getDistance(t2), 3.0f));
+    EXPECT_TRUE(approxEqual(t1.getDistanceSquared(t2), 9.0f));
+}
+
+TEST(TransformTests, EulerAndQuaternionConversion)
+{
+    Transform t(1, 2, 3, 0.1f, 0.2f, 0.3f);
+    float roll, pitch, yaw;
+    t.getEulerAngles(roll, pitch, yaw);
+    EXPECT_TRUE(std::isfinite(roll));
+    EXPECT_TRUE(std::isfinite(pitch));
+    EXPECT_TRUE(std::isfinite(yaw));
+
+    Eigen::Quaternionf q = t.getQuaternionf();
+    EXPECT_TRUE(std::isfinite(q.x()));
+    EXPECT_TRUE(std::isfinite(q.y()));
+    EXPECT_TRUE(std::isfinite(q.z()));
+    EXPECT_TRUE(std::isfinite(q.w()));
+}
+
+TEST(TransformTests, StringConversion)
+{
+    Transform original(1.0f, 2.0f, 3.0f, 0.1f, 0.2f, 0.3f);
+    Transform parsed = Transform::fromString("1.0 2.0 3.0 0.1 0.2 0.3");
+    EXPECT_TRUE(original.getDistance(parsed) < 1e-4f);
+}
+
+TEST(TransformTests, DoFConversion)
+{
+    // 6DoF transform
+    Transform t6(1, 2, 3, 0.1f, 0.2f, 0.3f);
+
+    Transform t3dof = t6.to3DoF();
+    EXPECT_TRUE(t3dof.is3DoF());
+    EXPECT_TRUE(t3dof.is4DoF());
+    EXPECT_TRUE(approxEqual(t3dof.z(), 0.0f)); // Z is removed
+    EXPECT_TRUE(approxEqual(t3dof.r31(), 0.0f));
+    EXPECT_TRUE(approxEqual(t3dof.r32(), 0.0f));
+    EXPECT_TRUE(approxEqual(t3dof.r33(), 1.0f));
+
+    Transform t4dof = t6.to4DoF();
+    EXPECT_TRUE(t4dof.is4DoF());
+    EXPECT_FALSE(t4dof.is3DoF());
+    EXPECT_TRUE(approxEqual(t4dof.r31(), 0.0f));
+    EXPECT_TRUE(approxEqual(t4dof.r32(), 0.0f));
+    EXPECT_TRUE(approxEqual(t4dof.r33(), 1.0f));
+}
+
+TEST(TransformTests, OpenGLToRTABMap)
+{
+    // Multiplying should return identity
+    Transform result = Transform::opengl_T_rtabmap() * Transform::rtabmap_T_opengl();
+    EXPECT_TRUE(result.isIdentity());
+
+    // Test conversion from opengl world to rtabmap world
+    EXPECT_NEAR((Transform::rtabmap_T_opengl() * Transform(1,0,0,0,0,0)).y(), -1, 1e-5);
+    EXPECT_NEAR((Transform::rtabmap_T_opengl() * Transform(0,1,0,0,0,0)).z(),  1, 1e-5);
+    EXPECT_NEAR((Transform::rtabmap_T_opengl() * Transform(0,0,1,0,0,0)).x(), -1, 1e-5);
+
+    // Test conversion from rtabmap world to opengl world
+    EXPECT_NEAR((Transform::opengl_T_rtabmap() * Transform(1,0,0,0,0,0)).z(), -1, 1e-5);
+    EXPECT_NEAR((Transform::opengl_T_rtabmap() * Transform(0,1,0,0,0,0)).x(), -1, 1e-5);
+    EXPECT_NEAR((Transform::opengl_T_rtabmap() * Transform(0,0,1,0,0,0)).y(),  1, 1e-5);
+}
+
+TEST(TransformTests, EigenConversions)
+{
+    Transform t(1.0f, 2.0f, 3.0f, 0.1f, 0.2f, 0.3f);
+
+    // Eigen 4x4 float
+    Eigen::Matrix4f e4f = t.toEigen4f();
+    Transform fromE4f = Transform::fromEigen4f(e4f);
+    EXPECT_LT(t.getDistance(fromE4f), 1e-4f);
+
+    // Eigen 4x4 double
+    Eigen::Matrix4d e4d = t.toEigen4d();
+    Transform fromE4d = Transform::fromEigen4d(e4d);
+    EXPECT_LT(t.getDistance(fromE4d), 1e-4f);
+
+    // Eigen Affine3f
+    Eigen::Affine3f aff3f = t.toEigen3f();
+    Transform fromAff3f = Transform::fromEigen3f(aff3f);
+    EXPECT_LT(t.getDistance(fromAff3f), 1e-4f);
+
+    // Eigen Affine3d
+    Eigen::Affine3d aff3d = t.toEigen3d();
+    Transform fromAff3d = Transform::fromEigen3d(aff3d);
+    EXPECT_LT(t.getDistance(fromAff3d), 1e-4f);
+
+    // Quaternion check
+    Eigen::Quaternionf qf = t.getQuaternionf();
+    EXPECT_NEAR(qf.norm(), 1.0f, 1e-5f);
+
+    Eigen::Quaterniond qd = t.getQuaterniond();
+    EXPECT_NEAR(qd.norm(), 1.0, 1e-5);
+}
+
+TEST(TransformTests, GetTransformFromBuffer)
+{
+    std::map<double, Transform> tfBuffer;
+
+    tfBuffer[1.0] = Transform(0, 0, 0, 0, 0, 0);         // t=1.0
+    tfBuffer[2.0] = Transform(1, 0, 0, 0, 0, M_PI/2.0);  // t=2.0
+    tfBuffer[3.0] = Transform(2, 0, 0, 0, 0, M_PI);      // t=3.0
+
+    // Test exact timestamp
+    Transform tExact = Transform::getTransform(tfBuffer, 2.0);
+    EXPECT_NEAR(tExact.x(), 1.0f, 1e-5);
+
+    // Test below
+    Transform tBelow = Transform::getTransform(tfBuffer, 1.5);
+    EXPECT_NEAR(tBelow.x(), 0.5f, 1e-5);
+    EXPECT_NEAR(tBelow.theta(), M_PI/4.0, 1e-5);
+
+    // Test above
+    Transform tAbove = Transform::getTransform(tfBuffer, 2.5);
+    EXPECT_NEAR(tAbove.x(), 1.5f, 1e-5);
+    EXPECT_NEAR(tAbove.theta(), 3.0*M_PI/4.0, 1e-5);
+
+    // Test very close to a border
+    Transform tNear = Transform::getTransform(tfBuffer, 2.0001);
+    EXPECT_NEAR(tNear.x(), 1.0001f, 1e-4);
+
+    // Requesting transform in the past
+    Transform tPast = Transform::getTransform(tfBuffer, 0.5);
+    EXPECT_TRUE(tPast.isNull());
+
+    // Requesting transform in the future
+    Transform tFuture = Transform::getTransform(tfBuffer, 3.5);
+    EXPECT_TRUE(tFuture.isNull());
+}

corelib/test/VisualWordTests.cpp

@@ -0,0 +1,82 @@
+#include <gtest/gtest.h>
+#include <opencv2/core.hpp>
+#include "rtabmap/core/VisualWord.h"
+#include <map>
+
+using namespace rtabmap;
+
+TEST(VisualWordTest, ConstructorAndAccessors)
+{
+    cv::Mat descriptor = (cv::Mat_<float>(1, 3) << 1.0, 2.0, 3.0);
+    int wordId = 123;
+    int signatureId = 10;
+
+    VisualWord word(wordId, descriptor, signatureId);
+
+    EXPECT_EQ(word.id(), wordId);
+    EXPECT_EQ(word.getTotalReferences(), 1);
+    EXPECT_EQ(word.getDescriptor().cols, descriptor.cols);
+    EXPECT_EQ(word.getDescriptor().rows, descriptor.rows);
+
+    auto refs = word.getReferences();
+    ASSERT_EQ(refs.size(), 1);
+    EXPECT_EQ(refs.at(signatureId), 1);
+}
+
+TEST(VisualWordTest, AddMultipleReferences)
+{
+    VisualWord word(1, cv::Mat::ones(1, 5, CV_32F));
+
+    word.addRef(100);
+    word.addRef(100); // same signature
+    word.addRef(200);
+
+    EXPECT_EQ(word.getTotalReferences(), 3);
+
+    auto refs = word.getReferences();
+    EXPECT_EQ(refs.size(), 2);
+    EXPECT_EQ(refs.at(100), 2);
+    EXPECT_EQ(refs.at(200), 1);
+}
+
+TEST(VisualWordTest, RemoveReferences)
+{
+    VisualWord word(1, cv::Mat::zeros(1, 3, CV_8U));
+    word.addRef(10);
+    word.addRef(10);
+    word.addRef(20);
+
+    EXPECT_EQ(word.getTotalReferences(), 3);
+
+    int removed = word.removeAllRef(10);
+    EXPECT_EQ(removed, 2);
+    EXPECT_EQ(word.getTotalReferences(), 1);
+
+    auto refs = word.getReferences();
+    EXPECT_EQ(refs.size(), 1);
+    EXPECT_TRUE(refs.find(10) == refs.end());
+    EXPECT_EQ(refs.at(20), 1);
+}
+
+TEST(VisualWordTest, SavedFlagBehavior)
+{
+    VisualWord word(1, cv::Mat());
+    EXPECT_FALSE(word.isSaved());
+
+    word.setSaved(true);
+    EXPECT_TRUE(word.isSaved());
+
+    word.setSaved(false);
+    EXPECT_FALSE(word.isSaved());
+}
+
+TEST(VisualWordTest, MemoryUsedEstimation)
+{
+    cv::Mat descriptor = cv::Mat::ones(1, 128, CV_32FC1); // simulate SIFT descriptor
+    VisualWord word(42, descriptor);
+
+    unsigned long mem = word.getMemoryUsed();
+
+    // Should be at least size of descriptor
+    EXPECT_GE(mem, 128ul*sizeof(float));
+}