Added utilite tests

utilite/test/CMakeLists.txt

@@ -0,0 +1,67 @@
+# UConversion.h
+add_executable(test_uconversion test_uconversion.cpp)
+target_link_libraries(test_uconversion gtest_main rtabmap::utilite)
+gtest_discover_tests(test_uconversion)
+
+# UTimer.h
+add_executable(test_utimer test_utimer.cpp)
+target_link_libraries(test_utimer gtest_main rtabmap::utilite)
+gtest_discover_tests(test_utimer)
+
+# UVariant.h
+add_executable(test_uvariant test_uvariant.cpp)
+target_link_libraries(test_uvariant gtest_main rtabmap::utilite)
+gtest_discover_tests(test_uvariant)
+
+# UFile.h
+add_executable(test_ufile test_ufile.cpp)
+target_link_libraries(test_ufile gtest_main rtabmap::utilite)
+gtest_discover_tests(test_ufile)
+
+# UDirectory.h
+add_executable(test_udirectory test_udirectory.cpp)
+target_link_libraries(test_udirectory gtest_main rtabmap::utilite)
+gtest_discover_tests(test_udirectory)
+
+# UProcessInfo.h
+add_executable(test_uprocessinfo test_uprocessinfo.cpp)
+target_link_libraries(test_uprocessinfo gtest_main rtabmap::utilite)
+gtest_discover_tests(test_uprocessinfo)
+
+# UMath.h
+add_executable(test_umath test_umath.cpp)
+target_link_libraries(test_umath gtest_main rtabmap::utilite)
+gtest_discover_tests(test_umath)
+
+# UStl.h
+add_executable(test_ustl test_ustl.cpp)
+target_link_libraries(test_ustl gtest_main rtabmap::utilite)
+gtest_discover_tests(test_ustl)
+
+# UEventsHandler.h, UEventsManager.h, UEventsSender.h
+add_executable(test_uevents test_uevents.cpp)
+target_link_libraries(test_uevents gtest_main rtabmap::utilite)
+gtest_discover_tests(test_uevents)
+
+# UMutex.h
+add_executable(test_umutex test_umutex.cpp)
+target_link_libraries(test_umutex gtest_main rtabmap::utilite)
+gtest_discover_tests(test_umutex)
+
+# UThread.h
+add_executable(test_uthread test_uthread.cpp)
+target_link_libraries(test_uthread gtest_main rtabmap::utilite)
+gtest_discover_tests(test_uthread)
+
+# USemaphore.h
+add_executable(test_usemaphore test_usemaphore.cpp)
+target_link_libraries(test_usemaphore gtest_main rtabmap::utilite)
+gtest_discover_tests(test_usemaphore)
+
+# ULogger.h
+add_executable(test_ulogger test_ulogger.cpp)
+target_link_libraries(test_ulogger gtest_main rtabmap::utilite)
+gtest_discover_tests(test_ulogger
+    PROPERTIES RUN_SERIAL TRUE
+)
+

utilite/test/test_uconversion.cpp

@@ -0,0 +1,203 @@
+#include "gtest/gtest.h"
+#include "rtabmap/utilite/UConversion.h"
+
+TEST(UConversionTest, uReplaceCharChar)
+{
+    std::string str = "Hello";
+    std::string result = uReplaceChar(str, 'l', 'p');
+    EXPECT_EQ(result, "Heppo");
+}
+
+TEST(UConversionTest, uReplaceCharString)
+{
+    std::string str = "Hello";
+    std::string result = uReplaceChar(str, 'o', "oween");
+    EXPECT_EQ(result, "Helloween");
+}
+
+TEST(UConversionTest, uToUpperCase)
+{
+    std::string str = "hello!";
+    std::string result = uToUpperCase(str);
+    EXPECT_EQ(result, "HELLO!");
+}
+
+TEST(UConversionTest, uToLowerCase)
+{
+    std::string str = "HELLO!";
+    std::string result = uToLowerCase(str);
+    EXPECT_EQ(result, "hello!");
+}
+
+TEST(UConversionTest, uNumber2StrUnsignedInt)
+{
+    unsigned int number = 42;
+    std::string result = uNumber2Str(number);
+    EXPECT_EQ(result, "42");
+}
+
+TEST(UConversionTest, uNumber2StrInt)
+{
+    int number = -42;
+    std::string result = uNumber2Str(number);
+    EXPECT_EQ(result, "-42");
+}
+
+TEST(UConversionTest, uNumber2StrFloat)
+{
+    float number = 0.14159f;
+    std::string result = uNumber2Str(number, 2, false);
+    EXPECT_STREQ(result.c_str(), "0.14");
+
+    number = 3.14159f;
+    result = uNumber2Str(number, 2, false);
+    EXPECT_STREQ(result.c_str(), "3.1");
+}
+
+TEST(UConversionTest, uNumber2StrDouble)
+{
+    double number = 0.14159;
+    std::string result = uNumber2Str(number, 2, false);
+    EXPECT_STREQ(result.c_str(), "0.14");
+
+    number = 3.14159;
+    result = uNumber2Str(number, 2, false);
+    EXPECT_STREQ(result.c_str(), "3.1");
+}
+
+TEST(UConversionTest, uStr2Int)
+{
+    std::string str = "42";
+    int result = uStr2Int(str);
+    EXPECT_EQ(result, 42);
+}
+
+TEST(UConversionTest, uStr2IntNegative)
+{
+    std::string str = "-42";
+    int result = uStr2Int(str);
+    EXPECT_EQ(result, -42);
+}
+
+TEST(UConversionTest, uStr2Float)
+{
+    std::string str = "3.14";
+    float result = uStr2Float(str);
+    EXPECT_NEAR(result, 3.14f, 0.001f);
+}
+
+TEST(UConversionTest, uStr2FloatComma)
+{
+    std::string str = "3,14";
+    float result = uStr2Float(str);
+    EXPECT_NEAR(result, 3.14f, 0.001f);
+}
+
+TEST(UConversionTest, uStr2Double)
+{
+    std::string str = "3.14159";
+    double result = uStr2Double(str);
+    EXPECT_NEAR(result, 3.14159, 0.00001);
+}
+
+TEST(UConversionTest, uStr2DoubleComma)
+{
+    std::string str = "3,14159";
+    double result = uStr2Double(str);
+    EXPECT_NEAR(result, 3.14159, 0.00001);
+}
+
+TEST(UConversionTest, uBool2Str)
+{
+    std::string resultTrue = uBool2Str(true);
+    std::string resultFalse = uBool2Str(false);
+    EXPECT_EQ(resultTrue, "true");
+    EXPECT_EQ(resultFalse, "false");
+}
+
+TEST(UConversionTest, uStr2Bool)
+{
+    EXPECT_TRUE(uStr2Bool("true"));
+    EXPECT_TRUE(uStr2Bool("TRUE"));
+    EXPECT_TRUE(uStr2Bool("True"));
+    EXPECT_FALSE(uStr2Bool("false"));
+    EXPECT_FALSE(uStr2Bool("FALSE"));
+    EXPECT_FALSE(uStr2Bool("0"));
+    EXPECT_TRUE(uStr2Bool("1"));
+    EXPECT_TRUE(uStr2Bool("anything"));
+}
+
+TEST(UConversionTest, uStr2Bytes)
+{
+    std::string str = "Hello";
+    std::vector<unsigned char> bytes = uStr2Bytes(str);
+    EXPECT_EQ(bytes.size(), 6); // "Hello" + null terminator
+    EXPECT_EQ(bytes[0], 'H');
+    EXPECT_EQ(bytes[4], 'o');
+    EXPECT_EQ(bytes[5], '\0');
+}
+
+TEST(UConversionTest, uBytes2Str)
+{
+    std::vector<unsigned char> bytes = {'H', 'e', 'l', 'l', 'o', '\0'};
+    std::string result = uBytes2Str(bytes);
+    EXPECT_EQ(result, "Hello");
+}
+
+TEST(UConversionTest, uBytes2Hex)
+{
+    char bytes[] = {0x3F, 0x1A};
+    std::string result = uBytes2Hex(bytes, 2);
+    EXPECT_EQ(result, "3F1A");
+}
+
+TEST(UConversionTest, uHex2Bytes)
+{
+    std::string hex = "1f3B";
+    std::vector<char> bytes = uHex2Bytes(hex);
+    EXPECT_EQ(bytes.size(), 2);
+    EXPECT_EQ(bytes[0], 0x1F);
+    EXPECT_EQ(bytes[1], 0x3B);
+}
+
+TEST(UConversionTest, uHex2BytesWithLength)
+{
+    std::string hex = "1f3B";
+    std::vector<char> bytes = uHex2Bytes(hex.c_str(), 4);
+    EXPECT_EQ(bytes.size(), 2);
+    EXPECT_EQ(bytes[0], 0x1F);
+    EXPECT_EQ(bytes[1], 0x3B);
+}
+
+TEST(UConversionTest, uHex2Str)
+{
+    std::string hex = "48656C6C6F21";
+    std::string result = uHex2Str(hex);
+    EXPECT_EQ(result, "Hello!");
+}
+
+TEST(UConversionTest, uHex2Ascii)
+{
+    unsigned char c = 0xFA;
+    unsigned char left = uHex2Ascii(c, false);
+    unsigned char right = uHex2Ascii(c, true);
+    EXPECT_EQ(left, 'F');
+    EXPECT_EQ(right, 'A');
+}
+
+TEST(UConversionTest, uAscii2Hex)
+{
+    unsigned char result = uAscii2Hex('F');
+    EXPECT_EQ(result, 0x0F);
+    result = uAscii2Hex('a');
+    EXPECT_EQ(result, 0x0A);
+}
+
+TEST(UConversionTest, uFormat)
+{
+    std::string result = uFormat("Hello %s %d", "world", 42);
+    EXPECT_NE(result.find("Hello"), std::string::npos);
+    EXPECT_NE(result.find("world"), std::string::npos);
+    EXPECT_NE(result.find("42"), std::string::npos);
+}
+

utilite/test/test_udirectory.cpp

@@ -0,0 +1,242 @@
+#include "gtest/gtest.h"
+#include "rtabmap/utilite/UDirectory.h"
+#include "rtabmap/utilite/UFile.h"
+#include <cstdio>
+#include <fstream>
+
+TEST(UDirectoryTest, Exists)
+{
+    // Test with current directory (should exist)
+    EXPECT_TRUE(UDirectory::exists("."));
+    
+    // Test with nonexistent directory
+    EXPECT_FALSE(UDirectory::exists("nonexistent_directory_xyz123"));
+}
+
+TEST(UDirectoryTest, GetDir)
+{
+    std::string filePath = "/path/to/file.txt";
+    std::string dir = UDirectory::getDir(filePath);
+    EXPECT_EQ(dir, "/path/to");
+
+    std::string filePath2 = "/path";
+    std::string dir2 = UDirectory::getDir(filePath2);
+    EXPECT_EQ(dir2, "/");
+    
+    std::string filePath3 = "file.txt";
+    std::string dir3 = UDirectory::getDir(filePath3);
+    EXPECT_EQ(dir3, ".");
+
+    std::string filePath4 = "./file.txt";
+    std::string dir4 = UDirectory::getDir(filePath4);
+    EXPECT_EQ(dir4, ".");
+}
+
+TEST(UDirectoryTest, CurrentDir)
+{
+    std::string current = UDirectory::currentDir(false);
+    EXPECT_GT(current.size(), 0);
+    
+    std::string currentWithSep = UDirectory::currentDir(true);
+    EXPECT_GT(currentWithSep.size(), 0);
+    // Should end with separator if trailingSeparator is true
+    std::string sep = UDirectory::separator();
+    if(currentWithSep.size() > 0)
+    {
+        EXPECT_EQ(currentWithSep.back(), sep[0]);
+    }
+}
+
+TEST(UDirectoryTest, MakeDir)
+{
+    std::string testDir = "test_make_dir";
+    // Remove if exists
+    if(UDirectory::exists(testDir))
+    {
+        UDirectory::removeDir(testDir);
+    }
+    
+    bool result = UDirectory::makeDir(testDir);
+    EXPECT_TRUE(result);
+    EXPECT_TRUE(UDirectory::exists(testDir));
+    
+    // Cleanup
+    UDirectory::removeDir(testDir);
+}
+
+TEST(UDirectoryTest, RemoveDir)
+{
+    std::string testDir = "test_remove_dir";
+    // Create if doesn't exist
+    if(!UDirectory::exists(testDir))
+    {
+        UDirectory::makeDir(testDir);
+    }
+    
+    bool result = UDirectory::removeDir(testDir);
+    EXPECT_TRUE(result);
+    EXPECT_FALSE(UDirectory::exists(testDir));
+}
+
+TEST(UDirectoryTest, HomeDir)
+{
+    std::string home = UDirectory::homeDir();
+    EXPECT_GT(home.size(), 0);
+}
+
+TEST(UDirectoryTest, Separator)
+{
+    std::string sep = UDirectory::separator();
+    EXPECT_EQ(sep.size(), 1);
+#ifdef _WIN32
+    EXPECT_EQ(sep, "\\");
+#else
+    EXPECT_EQ(sep, "/");
+#endif
+}
+
+TEST(UDirectoryTest, Constructor)
+{
+    std::string testDir = ".";
+    UDirectory dir(testDir);
+    EXPECT_TRUE(dir.isValid());
+}
+
+TEST(UDirectoryTest, SetPath)
+{
+    UDirectory dir;
+    dir.setPath(".");
+    EXPECT_TRUE(dir.isValid());
+}
+
+TEST(UDirectoryTest, GetNextFileName)
+{
+    // Create a test directory with files
+    std::string testDir = "test_dir_files";
+    if(!UDirectory::exists(testDir))
+    {
+        UDirectory::makeDir(testDir);
+    }
+    
+    // Create some test files
+    std::ofstream file1(testDir + UDirectory::separator() + "file1.txt");
+    file1 << "content1";
+    file1.close();
+    
+    std::ofstream file2(testDir + UDirectory::separator() + "file2.txt");
+    file2 << "content2";
+    file2.close();
+    
+    UDirectory dir(testDir);
+    std::string name1 = dir.getNextFileName();
+    std::string name2 = dir.getNextFileName();
+    
+    EXPECT_STREQ(name1.c_str(), "file1.txt");
+    EXPECT_STREQ(name2.c_str(), "file2.txt");
+    
+    // Cleanup
+    UFile::erase(testDir + UDirectory::separator() + "file1.txt");
+    UFile::erase(testDir + UDirectory::separator() + "file2.txt");
+    UDirectory::removeDir(testDir);
+}
+
+TEST(UDirectoryTest, GetNextFilePath)
+{
+    std::string testDir = "test_dir_paths";
+    if(!UDirectory::exists(testDir))
+    {
+        UDirectory::makeDir(testDir);
+    }
+    
+    std::ofstream file(testDir + UDirectory::separator() + "test.txt");
+    file << "content";
+    file.close();
+    
+    UDirectory dir(testDir);
+    std::string path = dir.getNextFilePath();
+    EXPECT_GT(path.size(), 0);
+    EXPECT_NE(path.find("test.txt"), std::string::npos);
+    
+    // Cleanup
+    UFile::erase(testDir + UDirectory::separator() + "test.txt");
+    UDirectory::removeDir(testDir);
+}
+
+TEST(UDirectoryTest, GetFileNames)
+{
+    std::string testDir = "test_dir_list";
+    if(!UDirectory::exists(testDir))
+    {
+        UDirectory::makeDir(testDir);
+    }
+    
+    std::ofstream file1(testDir + UDirectory::separator() + "file1.txt");
+    file1 << "content1";
+    file1.close();
+    
+    std::ofstream file2(testDir + UDirectory::separator() + "file2.txt");
+    file2 << "content2";
+    file2.close();
+    
+    UDirectory dir(testDir);
+    const std::list<std::string>& fileNames = dir.getFileNames();
+    EXPECT_GE(fileNames.size(), 2);
+    EXPECT_STREQ(fileNames.front().c_str(), "file1.txt");
+    EXPECT_STREQ(fileNames.back().c_str(), "file2.txt");
+    
+    // Cleanup
+    UFile::erase(testDir + UDirectory::separator() + "file1.txt");
+    UFile::erase(testDir + UDirectory::separator() + "file2.txt");
+    UDirectory::removeDir(testDir);
+}
+
+TEST(UDirectoryTest, Rewind)
+{
+    std::string testDir = "test_dir_rewind";
+    if(UDirectory::exists(testDir))
+    {
+        UDirectory::removeDir(testDir);
+    }
+    UDirectory::makeDir(testDir);
+    std::ofstream file1(testDir + UDirectory::separator() + "file1.txt");
+    file1 << "content1";
+    file1.close();
+    UDirectory dir(testDir);
+    std::string name1 = dir.getNextFileName();
+    EXPECT_FALSE(name1.empty());
+    dir.rewind();
+    std::string name2 = dir.getNextFileName();
+    // Should get the same file name after rewind
+    EXPECT_EQ(name1, name2);
+}
+
+TEST(UDirectoryTest, ExtensionsFilter)
+{
+    std::string testDir = "test_dir_ext";
+    if(!UDirectory::exists(testDir))
+    {
+        UDirectory::makeDir(testDir);
+    }
+    
+    std::ofstream file1(testDir + UDirectory::separator() + "file1.txt");
+    file1 << "content1";
+    file1.close();
+    
+    std::ofstream file2(testDir + UDirectory::separator() + "file2.jpg");
+    file2 << "content2";
+    file2.close();
+    
+    UDirectory dir(testDir, "txt");
+    const std::list<std::string>& fileNames = dir.getFileNames();
+    // Should only get .txt files
+    for(const std::string& name : fileNames)
+    {
+        EXPECT_NE(name.find(".txt"), std::string::npos);
+    }
+    
+    // Cleanup
+    UFile::erase(testDir + UDirectory::separator() + "file1.txt");
+    UFile::erase(testDir + UDirectory::separator() + "file2.jpg");
+    UDirectory::removeDir(testDir);
+}
+

utilite/test/test_uevents.cpp

@@ -0,0 +1,478 @@
+#include "gtest/gtest.h"
+#include "rtabmap/utilite/UEvent.h"
+#include "rtabmap/utilite/UEventsHandler.h"
+#include "rtabmap/utilite/UEventsManager.h"
+#include "rtabmap/utilite/UEventsSender.h"
+#include "rtabmap/utilite/UTimer.h"
+#include <thread>
+#include <chrono>
+#include <atomic>
+
+// Test event classes
+class TestEvent : public UEvent
+{
+public:
+    TestEvent(int code = 0) : UEvent(code) {}
+    virtual ~TestEvent() {}
+    virtual std::string getClassName() const {return "TestEvent";}
+};
+
+class AnotherEvent : public UEvent
+{
+public:
+    AnotherEvent() : UEvent(0) {}
+    virtual ~AnotherEvent() {}
+    virtual std::string getClassName() const {return "AnotherEvent";}
+};
+
+// Test handler class
+class TestHandler : public UEventsHandler
+{
+public:
+    TestHandler() : eventCount_(0), lastEventCode_(0){}
+    
+    int getEventCount() const { return eventCount_; }
+    int getLastEventCode() const { return lastEventCode_; }
+    const std::string & getLastEventClassName() const { return lastEventClassName_; }
+    void reset() { eventCount_ = 0; lastEventCode_ = 0; lastEventClassName_.clear(); }
+    
+protected:
+    virtual bool handleEvent(UEvent * event)
+    {
+        eventCount_++;
+        lastEventCode_ = event->getCode();
+        lastEventClassName_ = event->getClassName();
+        return false;
+    }
+
+private:
+    int eventCount_;
+    int lastEventCode_;
+    std::string lastEventClassName_;
+    bool ownershipTaken_;
+};
+
+// Handler that takes ownership
+class OwnershipHandler : public UEventsHandler
+{
+public:
+    OwnershipHandler() : event_(nullptr) {}
+    ~OwnershipHandler() {delete event_;}
+    const UEvent * getEvent() const {return event_;} 
+    void reset() { delete event_; event_ = nullptr;}
+    
+protected:
+    virtual bool handleEvent(UEvent * event)
+    {
+        event_ = event; // Take ownership
+        return true;
+    }
+    
+private:
+    UEvent * event_;
+};
+
+// Handler that filters events
+class FilteringHandler : public UEventsHandler
+{
+public:
+    FilteringHandler() : testEventCount_(0), anotherEventCount_(0) {}
+    int getTestEventCount() const { return testEventCount_; }
+    int getAnotherEventCount() const { return anotherEventCount_; }
+    void reset() { testEventCount_ = 0; anotherEventCount_ = 0; }
+    
+protected:
+    virtual bool handleEvent(UEvent * event)
+    {
+        if(event->getClassName() == "TestEvent")
+        {
+            testEventCount_++;
+        }
+        else if(event->getClassName() == "AnotherEvent")
+        {
+            anotherEventCount_++;
+        }
+        return false;
+    }
+    
+private:
+    int testEventCount_;
+    int anotherEventCount_;
+};
+
+// Test sender class that can post events
+class TestSender : public UEventsSender
+{
+public:
+    TestSender() {}
+    
+    // Public method to test protected post() method
+    void testPost(UEvent* event, bool async = true) const
+    {
+        post(event, async);
+    }
+};
+
+TEST(UEventsTest, UEventGetClassName)
+{
+    TestEvent event;
+    EXPECT_EQ(event.getClassName(), "TestEvent");
+    EXPECT_EQ(event.getCode(), 0);
+}
+
+TEST(UEventsTest, UEventGetCode)
+{
+    TestEvent event(42);
+    EXPECT_EQ(event.getCode(), 42);
+}
+
+TEST(UEventsTest, UEventsHandlerRegisterUnregister)
+{
+    TestHandler handler;
+    
+    // Register handler
+    handler.registerToEventsManager();
+    
+    // Post an event
+    UEventsManager::post(new TestEvent(), true);
+    
+    // Give time for event to be processed
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    
+    EXPECT_GT(handler.getEventCount(), 0);
+    
+    // Unregister handler
+    handler.unregisterFromEventsManager();
+    
+    // Post another event
+    int countBefore = handler.getEventCount();
+    UEventsManager::post(new TestEvent(), true);
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    
+    // Should not receive event after unregister
+    EXPECT_EQ(handler.getEventCount(), countBefore);
+}
+
+TEST(UEventsTest, UEventsHandlerDestructorUnregisters)
+{
+    TestHandler* handler = new TestHandler();
+    handler->registerToEventsManager();
+    
+    // Post an event
+    UEventsManager::post(new TestEvent(), true);
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+
+    EXPECT_EQ(handler->getEventCount(), 1);
+    
+    // Delete handler (should unregister automatically)
+    handler->unregisterFromEventsManager();
+    
+    // Post another event
+    UEventsManager::post(new TestEvent(), true);
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    
+    EXPECT_EQ(handler->getEventCount(), 1);
+}
+
+TEST(UEventsTest, UEventsManagerAddRemoveHandler)
+{
+    TestHandler handler1;
+    TestHandler handler2;
+    
+    UEventsManager::addHandler(&handler1);
+    UEventsManager::addHandler(&handler2);
+    
+    UEventsManager::post(new TestEvent(), true);
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    
+    EXPECT_GT(handler1.getEventCount(), 0);
+    EXPECT_GT(handler2.getEventCount(), 0);
+    
+    UEventsManager::removeHandler(&handler1);
+    
+    int count1Before = handler1.getEventCount();
+    int count2Before = handler2.getEventCount();
+    
+    UEventsManager::post(new TestEvent(), true);
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    
+    EXPECT_EQ(handler1.getEventCount(), count1Before);
+    EXPECT_GT(handler2.getEventCount(), count2Before);
+    
+    UEventsManager::removeHandler(&handler2);
+}
+
+TEST(UEventsTest, UEventsManagerPostAsync)
+{
+    TestHandler handler;
+    UEventsManager::addHandler(&handler);
+    
+    handler.reset();
+    
+    UEventsManager::post(new TestEvent(), true);
+    
+    // Give time for async processing
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    
+    EXPECT_GT(handler.getEventCount(), 0);
+    
+    UEventsManager::removeHandler(&handler);
+}
+
+TEST(UEventsTest, UEventsManagerPostSync)
+{
+    TestHandler handler;
+    UEventsManager::addHandler(&handler);
+    
+    handler.reset();
+    
+    UEventsManager::post(new TestEvent(), false);
+    
+    // Sync should be immediate
+    EXPECT_GT(handler.getEventCount(), 0);
+    
+    UEventsManager::removeHandler(&handler);
+}
+
+TEST(UEventsTest, UEventsManagerMultipleHandlers)
+{
+    TestHandler handler1;
+    TestHandler handler2;
+    TestHandler handler3;
+    
+    UEventsManager::addHandler(&handler1);
+    UEventsManager::addHandler(&handler2);
+    UEventsManager::addHandler(&handler3);
+    
+    handler1.reset();
+    handler2.reset();
+    handler3.reset();
+    
+    UEventsManager::post(new TestEvent(), true);
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    
+    EXPECT_GT(handler1.getEventCount(), 0);
+    EXPECT_GT(handler2.getEventCount(), 0);
+    EXPECT_GT(handler3.getEventCount(), 0);
+    
+    UEventsManager::removeHandler(&handler1);
+    UEventsManager::removeHandler(&handler2);
+    UEventsManager::removeHandler(&handler3);
+}
+
+TEST(UEventsTest, UEventsManagerEventOrdering)
+{
+    TestHandler handler;
+    UEventsManager::addHandler(&handler);
+    
+    handler.reset();
+    
+    // Post multiple events
+    UEventsManager::post(new TestEvent(1), true);
+    UEventsManager::post(new TestEvent(2), true);
+    UEventsManager::post(new TestEvent(3), true);
+    
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    
+    // Should receive all events (order may vary in async mode)
+    EXPECT_GE(handler.getEventCount(), 3);
+    
+    UEventsManager::removeHandler(&handler);
+}
+
+TEST(UEventsTest, UEventsHandlerOwnership)
+{
+    OwnershipHandler handler;
+    TestHandler handler0;
+    TestHandler handler2;
+    UEventsManager::addHandler(&handler0);
+    UEventsManager::addHandler(&handler);
+    UEventsManager::addHandler(&handler2);
+    
+    UEventsManager::post(new TestEvent(42), true);
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    
+    EXPECT_NE(handler.getEvent(), nullptr);
+    EXPECT_EQ(handler.getEvent()->getCode(), 42);
+
+    // Should have received the event
+    EXPECT_GT(handler0.getEventCount(), 0);
+
+    // Should not have received the event
+    EXPECT_EQ(handler2.getEventCount(), 0);
+    
+    UEventsManager::removeHandler(&handler0);
+    UEventsManager::removeHandler(&handler);
+    UEventsManager::removeHandler(&handler2);
+}
+
+TEST(UEventsTest, UEventsHandlerFiltering)
+{
+    FilteringHandler handler;
+    UEventsManager::addHandler(&handler);
+    
+    handler.reset();
+    
+    UEventsManager::post(new TestEvent(), true);
+    UEventsManager::post(new AnotherEvent(), true);
+    UEventsManager::post(new TestEvent(), true);
+    
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    
+    EXPECT_EQ(handler.getTestEventCount(), 2);
+    EXPECT_EQ(handler.getAnotherEventCount(), 1);
+    
+    UEventsManager::removeHandler(&handler);
+}
+
+TEST(UEventsTest, UEventsSenderPost)
+{
+    TestSender sender;
+    TestHandler handler;
+    
+    UEventsManager::addHandler(&handler);
+    
+    handler.reset();
+    
+    // Test protected post() method through TestSender
+    sender.testPost(new TestEvent(), true);
+    
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    
+    EXPECT_GT(handler.getEventCount(), 0);
+    
+    UEventsManager::removeHandler(&handler);
+}
+
+TEST(UEventsTest, UEventsSenderDestructorRemovesPipes)
+{
+    TestSender* sender = new TestSender();
+    TestHandler handler;
+    
+    UEventsManager::addHandler(&handler);
+    UEventsManager::createPipe(sender, &handler, "TestEvent");
+    
+    // Delete sender - should remove pipes automatically
+    delete sender;
+    
+    // Should not crash and pipes should be removed
+    SUCCEED();
+    
+    UEventsManager::removeHandler(&handler);
+}
+
+TEST(UEventsTest, UEventsManagerCreateRemovePipe)
+{
+    TestSender sender;
+    TestHandler handler;
+    TestHandler handler2;
+    
+    UEventsManager::addHandler(&handler);
+    UEventsManager::addHandler(&handler2);
+    
+    // Create a pipe
+    UEventsManager::createPipe(&sender, &handler, "TestEvent");
+    
+    handler.reset();
+    
+    // Post event with sender
+    sender.testPost(new TestEvent());
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    EXPECT_EQ(handler.getEventCount(), 1);
+    EXPECT_EQ(handler2.getEventCount(), 0);
+
+    // Post event with sender using UEventsManager
+    UEventsManager::post(new TestEvent(), true, &sender);
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    EXPECT_EQ(handler.getEventCount(), 2);
+    EXPECT_EQ(handler2.getEventCount(), 0);
+
+    // Post global event
+    UEventsManager::post(new TestEvent(), true);
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    EXPECT_EQ(handler.getEventCount(), 3);
+    EXPECT_EQ(handler2.getEventCount(), 1);
+    
+    // Remove pipe
+    UEventsManager::removePipe(&sender, &handler, "TestEvent");
+    
+
+    // Post another event
+    UEventsManager::post(new TestEvent(), true, &sender);
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    
+    // Should still receive events, no pipe filtering
+    EXPECT_EQ(handler.getEventCount(), 4);
+    EXPECT_EQ(handler2.getEventCount(), 2);
+    
+    UEventsManager::removeHandler(&handler);
+    UEventsManager::removeHandler(&handler2);
+}
+
+TEST(UEventsTest, UEventsManagerRemoveAllPipes)
+{
+    TestSender sender;
+    TestHandler handler1;
+    TestHandler handler2;
+    
+    UEventsManager::addHandler(&handler1);
+    UEventsManager::addHandler(&handler2);
+    
+    UEventsManager::createPipe(&sender, &handler1, "TestEvent");
+    UEventsManager::createPipe(&sender, &handler2, "AnotherEvent");
+
+    sender.testPost(new TestEvent());
+    sender.testPost(new AnotherEvent());
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    EXPECT_EQ(handler1.getEventCount(), 1);
+    EXPECT_EQ(handler2.getEventCount(), 1);
+    
+    UEventsManager::removeAllPipes(&sender);
+    
+    sender.testPost(new TestEvent());
+    sender.testPost(new AnotherEvent());
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    EXPECT_EQ(handler1.getEventCount(), 3);
+    EXPECT_EQ(handler2.getEventCount(), 3);
+    
+    UEventsManager::removeHandler(&handler1);
+    UEventsManager::removeHandler(&handler2);
+}
+
+TEST(UEventsTest, MultipleEventTypes)
+{
+    FilteringHandler handler;
+    UEventsManager::addHandler(&handler);
+    
+    handler.reset();
+    
+    UEventsManager::post(new TestEvent(), true);
+    UEventsManager::post(new AnotherEvent(), true);
+    UEventsManager::post(new TestEvent(), true);
+    
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    
+    EXPECT_EQ(handler.getTestEventCount(), 2);
+    EXPECT_EQ(handler.getAnotherEventCount(), 1);
+    
+    UEventsManager::removeHandler(&handler);
+}
+
+TEST(UEventsTest, HandlerReceivesCorrectEvent)
+{
+    TestHandler handler;
+    UEventsManager::addHandler(&handler);
+    
+    handler.reset();
+    
+    TestEvent* event = new TestEvent(42);
+    UEventsManager::post(event, true);
+    
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    
+    EXPECT_EQ(handler.getLastEventCode(), 42);
+    EXPECT_STREQ(handler.getLastEventClassName().c_str(), "TestEvent");
+    
+    UEventsManager::removeHandler(&handler);
+}
+

utilite/test/test_ufile.cpp

@@ -0,0 +1,145 @@
+#include "gtest/gtest.h"
+#include "rtabmap/utilite/UFile.h"
+#include "rtabmap/utilite/UDirectory.h"
+#include <fstream>
+#include <cstdio>
+
+TEST(UFileTest, Exists)
+{
+    // Create a temporary file
+    std::string testFile = "test_file_exists.txt";
+    std::ofstream file(testFile);
+    file << "test content";
+    file.close();
+    
+    EXPECT_TRUE(UFile::exists(testFile));
+    EXPECT_FALSE(UFile::exists("nonexistent_file.txt"));
+    
+    // Cleanup
+    std::remove(testFile.c_str());
+}
+
+TEST(UFileTest, Length)
+{
+    std::string testFile = "test_file_length.txt";
+    std::string content = "test content";
+    std::ofstream file(testFile);
+    file << content;
+    file.close();
+    
+    long length = UFile::length(testFile);
+    EXPECT_EQ(length, static_cast<long>(content.size()));
+    
+    long lengthNonexistent = UFile::length("nonexistent_file.txt");
+    EXPECT_EQ(lengthNonexistent, 0);
+    
+    // Cleanup
+    std::remove(testFile.c_str());
+}
+
+TEST(UFileTest, Erase)
+{
+    std::string testFile = "test_file_erase.txt";
+    std::ofstream file(testFile);
+    file << "test content";
+    file.close();
+    
+    EXPECT_TRUE(UFile::exists(testFile));
+    int result = UFile::erase(testFile);
+    EXPECT_EQ(result, 0);
+    EXPECT_FALSE(UFile::exists(testFile));
+}
+
+TEST(UFileTest, Rename)
+{
+    std::string oldFile = "test_file_old.txt";
+    std::string newFile = "test_file_new.txt";
+    std::ofstream file(oldFile);
+    file << "test content";
+    file.close();
+    
+    int result = UFile::rename(oldFile, newFile);
+    EXPECT_EQ(result, 0);
+    EXPECT_FALSE(UFile::exists(oldFile));
+    EXPECT_TRUE(UFile::exists(newFile));
+    
+    // Cleanup
+    std::remove(newFile.c_str());
+}
+
+TEST(UFileTest, GetName)
+{
+    std::string path = "/path/to/file.txt";
+    std::string name = UFile::getName(path);
+    EXPECT_EQ(name, "file.txt");
+    
+    std::string path2 = "file.txt";
+    std::string name2 = UFile::getName(path2);
+    EXPECT_EQ(name2, "file.txt");
+}
+
+TEST(UFileTest, GetExtension)
+{
+    std::string path = "/path/to/file.txt";
+    std::string ext = UFile::getExtension(path);
+    EXPECT_EQ(ext, "txt");
+    
+    std::string path2 = "file";
+    std::string ext2 = UFile::getExtension(path2);
+    EXPECT_STREQ(ext2.c_str(), "");
+}
+
+TEST(UFileTest, Copy)
+{
+    std::string sourceFile = "test_file_source.txt";
+    std::string destFile = "test_file_dest.txt";
+    std::string content = "test content";
+    
+    std::ofstream file(sourceFile);
+    file << content;
+    file.close();
+    
+    UFile::copy(sourceFile, destFile);
+    EXPECT_TRUE(UFile::exists(destFile));
+    EXPECT_EQ(UFile::length(destFile), static_cast<long>(content.size()));
+    
+    // Cleanup
+    std::remove(sourceFile.c_str());
+    std::remove(destFile.c_str());
+}
+
+TEST(UFileTest, InstanceMethods)
+{
+    std::string testFile = "test_file_instance.txt";
+    std::ofstream file(testFile);
+    file << "test content";
+    file.close();
+    
+    UFile ufile(testFile);
+    EXPECT_TRUE(ufile.isValid());
+    EXPECT_TRUE(ufile.exists());
+    EXPECT_GT(ufile.length(), 0);
+    EXPECT_EQ(ufile.getName(), "test_file_instance.txt");
+    EXPECT_EQ(ufile.getExtension(), "txt");
+    
+    // Cleanup
+    std::remove(testFile.c_str());
+}
+
+TEST(UFileTest, InstanceRename)
+{
+    std::string testFile = "test_file_rename_old.txt";
+    std::ofstream file(testFile);
+    file << "test content";
+    file.close();
+    
+    UFile ufile(testFile);
+    int result = ufile.rename("test_file_rename_new");
+    EXPECT_EQ(result, 0);
+    EXPECT_FALSE(UFile::exists(testFile));
+    EXPECT_TRUE(UFile::exists("test_file_rename_new.txt"));
+    
+    // Cleanup
+    std::remove("test_file_rename_new.txt");
+}
+

utilite/test/test_ulogger.cpp

@@ -0,0 +1,774 @@
+#include "gtest/gtest.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UFile.h"
+#include "rtabmap/utilite/UThread.h"
+#include "rtabmap/utilite/UStl.h"
+#include <fstream>
+#include <sstream>
+#include <thread>
+
+TEST(ULoggerTest, DefaultState)
+{
+    ULogger::reset();
+    
+    EXPECT_EQ(ULogger::type(), ULogger::kTypeNoLog);
+    EXPECT_EQ(ULogger::level(), ULogger::kInfo);
+    EXPECT_TRUE(ULogger::isPrintTime());
+    EXPECT_TRUE(ULogger::isPrintLevel());
+    EXPECT_TRUE(ULogger::isPrintEndLine());
+    EXPECT_TRUE(ULogger::isPrintColored());
+    EXPECT_TRUE(ULogger::isPrintWhere());
+    EXPECT_FALSE(ULogger::isPrintWhereFullPath());
+    EXPECT_FALSE(ULogger::isPrintThreadId());
+    EXPECT_FALSE(ULogger::isBuffered());
+}
+
+TEST(ULoggerTest, SetType)
+{
+    ULogger::setType(ULogger::kTypeConsole);
+    EXPECT_EQ(ULogger::type(), ULogger::kTypeConsole);
+    
+    ULogger::setType(ULogger::kTypeNoLog);
+    EXPECT_EQ(ULogger::type(), ULogger::kTypeNoLog);
+    
+    ULogger::setType(ULogger::kTypeFile, "test_log.txt", false);
+    EXPECT_EQ(ULogger::type(), ULogger::kTypeFile);
+    
+    // Cleanup
+    UFile::erase("test_log.txt");
+}
+
+TEST(ULoggerTest, SetLevel)
+{
+    ULogger::setLevel(ULogger::kDebug);
+    EXPECT_EQ(ULogger::level(), ULogger::kDebug);
+    
+    ULogger::setLevel(ULogger::kInfo);
+    EXPECT_EQ(ULogger::level(), ULogger::kInfo);
+    
+    ULogger::setLevel(ULogger::kWarning);
+    EXPECT_EQ(ULogger::level(), ULogger::kWarning);
+    
+    ULogger::setLevel(ULogger::kError);
+    EXPECT_EQ(ULogger::level(), ULogger::kError);
+    
+    ULogger::setLevel(ULogger::kFatal);
+    EXPECT_EQ(ULogger::level(), ULogger::kFatal);
+}
+
+TEST(ULoggerTest, LevelFiltering)
+{
+    ULogger::reset();
+    std::string testFile = "logger_level_filtering_test.txt";
+    ULogger::setType(ULogger::kTypeFile, testFile, false);
+
+    ULogger::setLevel(ULogger::kInfo);
+    
+    // Debug should not be logged
+    UDEBUG("This debug message should not appear");
+    
+    // Info and above should be logged
+    UINFO("This info message should appear");
+    UWARN("This warning message should appear");
+    UERROR("This error message should appear");
+
+    // Make sure the log file is closed
+    ULogger::setType(ULogger::kTypeNoLog);
+
+    std::ifstream file(testFile);
+    std::string fileContent(
+        (std::istreambuf_iterator<char>(file)), // Start iterator
+        std::istreambuf_iterator<char>()        // End iterator
+    );
+    file.close();
+
+    EXPECT_FALSE(uStrContains(fileContent, "DEBUG"));
+    EXPECT_TRUE(uStrContains(fileContent, "INFO"));
+    EXPECT_TRUE(uStrContains(fileContent, "WARN"));
+    EXPECT_TRUE(uStrContains(fileContent, "ERROR"));
+}
+
+TEST(ULoggerTest, SetPrintTime)
+{
+    ULogger::reset();
+
+    std::string testFile = "logger_print_time_test.txt";
+    ULogger::setType(ULogger::kTypeFile, testFile, false);
+
+    ULogger::setPrintTime(false);
+    EXPECT_FALSE(ULogger::isPrintTime());
+    UINFO("This info message should appear without time");
+    
+    ULogger::setPrintTime(true);
+    EXPECT_TRUE(ULogger::isPrintTime());
+    UINFO("This info message should appear with time");
+
+    // Make sure the log file is closed
+    ULogger::setType(ULogger::kTypeNoLog);
+
+    std::ifstream file(testFile);
+    std::string line;
+    EXPECT_TRUE(file.is_open());
+
+    // First line should not have date
+    std::getline(file, line);
+    EXPECT_EQ(uSplit(line, '-').size(), 1);
+    // Time is format YYYY-MM-DD HH:MM:SS
+    std::getline(file, line);
+    EXPECT_EQ(uSplit(line, '-').size(), 3);
+    file.close(); 
+}
+
+TEST(ULoggerTest, SetPrintLevel)
+{
+    ULogger::reset();
+
+    std::string testFile = "logger_print_level_test.txt";
+    ULogger::setType(ULogger::kTypeFile, testFile, false);
+
+    ULogger::setPrintLevel(false);
+    EXPECT_FALSE(ULogger::isPrintLevel());
+    UINFO("This info message should appear without level");
+    
+    ULogger::setPrintLevel(true);
+    EXPECT_TRUE(ULogger::isPrintLevel());
+    UINFO("This info message should appear with level");
+
+    // Make sure the log file is closed
+    ULogger::setType(ULogger::kTypeNoLog);
+
+    std::ifstream file(testFile);
+    std::string line;
+    EXPECT_TRUE(file.is_open());
+
+    // First line should not have level
+    std::getline(file, line);
+    EXPECT_EQ(uSplit(line, ']').size(), 1);
+    // Level format: [INFO] msg...
+    std::getline(file, line);
+    EXPECT_EQ(uSplit(line, ']').size(), 2);
+    file.close(); 
+}
+
+TEST(ULoggerTest, SetPrintEndline)
+{
+    ULogger::reset();
+    std::string testFile = "logger_print_endline_test.txt";
+    ULogger::setType(ULogger::kTypeFile, testFile, false);
+    
+    ULogger::setPrintEndline(false);
+    EXPECT_FALSE(ULogger::isPrintEndLine());
+    UINFO("This info message should appear first. ");
+    
+    ULogger::setPrintEndline(true);
+    EXPECT_TRUE(ULogger::isPrintEndLine());
+    UINFO("This info message should appear on same line than previous msg.");
+
+    // Make sure the log file is closed
+    ULogger::setType(ULogger::kTypeNoLog);
+
+    std::ifstream file(testFile);
+    std::string line;
+    EXPECT_TRUE(file.is_open());
+
+    // First line should not have level
+    std::getline(file, line);
+    EXPECT_FALSE(line.empty());
+    std::getline(file, line);
+    EXPECT_TRUE(line.empty());
+    file.close(); 
+}
+
+TEST(ULoggerTest, SetPrintColored)
+{
+    ULogger::reset();
+    
+    ULogger::setPrintColored(false);
+    EXPECT_FALSE(ULogger::isPrintColored());
+    
+    ULogger::setPrintColored(true);
+    EXPECT_TRUE(ULogger::isPrintColored());
+}
+
+TEST(ULoggerTest, SetPrintWhere)
+{
+    ULogger::reset();
+    std::string testFile = "logger_print_where_test.txt";
+    ULogger::setType(ULogger::kTypeFile, testFile, false);
+    
+    ULogger::setPrintWhere(false);
+    EXPECT_FALSE(ULogger::isPrintWhere());
+    UINFO("This info message should appear without where");
+    
+    ULogger::setPrintWhere(true);
+    EXPECT_TRUE(ULogger::isPrintWhere());
+    UINFO("This info message should appear with where");
+
+    // Make sure the log file is closed
+    ULogger::setType(ULogger::kTypeNoLog);
+
+    std::ifstream file(testFile);
+    std::string line;
+    EXPECT_TRUE(file.is_open());
+
+    // First line should not have where (no test_ulogger.cpp)
+    std::getline(file, line);
+    EXPECT_FALSE(uStrContains(line, "test_ulogger.cpp"));
+    std::getline(file, line);
+    EXPECT_TRUE(uStrContains(line, "test_ulogger.cpp"));
+    file.close(); 
+}
+
+TEST(ULoggerTest, SetPrintWhereFullPath)
+{
+    ULogger::reset();
+    std::string testFile = "logger_print_where_full_test.txt";
+    ULogger::setType(ULogger::kTypeFile, testFile, false);
+    
+    ULogger::setPrintWhereFullPath(false);
+    EXPECT_FALSE(ULogger::isPrintWhereFullPath());
+    UINFO("This info message should appear without full path");
+    
+    ULogger::setPrintWhereFullPath(true);
+    EXPECT_TRUE(ULogger::isPrintWhereFullPath());
+    UINFO("This info message should appear with full path");
+
+    // Make sure the log file is closed
+    ULogger::setType(ULogger::kTypeNoLog);
+
+    std::ifstream file(testFile);
+    std::string line;
+    EXPECT_TRUE(file.is_open());
+
+    // First line should not have full path (without / or \\)
+    std::getline(file, line);
+    EXPECT_TRUE(!uStrContains(line, "/") && !uStrContains(line, "\\"));
+    std::getline(file, line);
+    EXPECT_TRUE(uStrContains(line, "/") || uStrContains(line, "\\"));
+    file.close(); 
+}
+
+TEST(ULoggerTest, SetPrintThreadId)
+{
+    ULogger::reset();
+    std::string testFile = "logger_print_thread_id_test.txt";
+    ULogger::setType(ULogger::kTypeFile, testFile, false);
+    
+    ULogger::setPrintThreadId(false);
+    EXPECT_FALSE(ULogger::isPrintThreadId());
+    UINFO("This info message should appear without thread ID");
+    
+    ULogger::setPrintThreadId(true);
+    EXPECT_TRUE(ULogger::isPrintThreadId());
+    UINFO("This info message should appear with thread ID");
+
+    // Make sure the log file is closed
+    ULogger::setType(ULogger::kTypeNoLog);
+
+    std::ifstream file(testFile);
+    std::string line;
+    EXPECT_TRUE(file.is_open());
+
+    // First line should not have thread ID (without {})
+    std::getline(file, line);
+    EXPECT_TRUE(!uStrContains(line, "{") && !uStrContains(line, "}"));
+    std::getline(file, line);
+    EXPECT_TRUE(uStrContains(line, "{") && uStrContains(line, "}"));
+    file.close(); 
+}
+
+TEST(ULoggerTest, SetBuffered)
+{
+    ULogger::reset();
+    ULogger::setBuffered(true);
+    EXPECT_TRUE(ULogger::isBuffered());
+    
+    ULogger::setBuffered(false);
+    EXPECT_FALSE(ULogger::isBuffered());
+}
+
+TEST(ULoggerTest, FileLogging)
+{
+    ULogger::reset();
+    std::string testFile = "test_ulogger_file.txt";
+    
+    // Remove file if exists
+    if(UFile::exists(testFile))
+    {
+        UFile::erase(testFile);
+    }
+    
+    ULogger::setType(ULogger::kTypeFile, testFile, false);
+    ULogger::setLevel(ULogger::kDebug);
+    
+    UINFO("Test message 1");
+    UWARN("Test message 2");
+    UERROR("Test message 3");
+    
+    // Make sure everything is written to file
+    ULogger::setType(ULogger::kTypeNoLog);
+    
+    // Check file exists and has content
+    EXPECT_TRUE(UFile::exists(testFile));
+    EXPECT_GT(UFile::length(testFile), 0);
+}
+
+TEST(ULoggerTest, FileLoggingAppend)
+{
+    ULogger::reset();
+    std::string testFile = "test_ulogger_append.txt";
+    
+    // Remove file if exists
+    if(UFile::exists(testFile))
+    {
+        UFile::erase(testFile);
+    }
+    
+    ULogger::setType(ULogger::kTypeFile, testFile, false);
+    ULogger::setLevel(ULogger::kInfo);
+    
+    UINFO("First message");
+    // Make sure everything is written to file
+    ULogger::setType(ULogger::kTypeNoLog);
+    
+    long length1 = UFile::length(testFile);
+    
+    // Append mode
+    ULogger::setType(ULogger::kTypeFile, testFile, true);
+    UINFO("Second message");
+    // Make sure everything is written to file
+    ULogger::setType(ULogger::kTypeNoLog);
+    
+    long length2 = UFile::length(testFile);
+    EXPECT_GT(length2, length1);
+}
+
+TEST(ULoggerTest, BufferedLogging)
+{
+    ULogger::reset();
+    std::string testFile = "test_ulogger_buffered.txt";
+    
+    // Remove file if exists
+    if(UFile::exists(testFile))
+    {
+        UFile::erase(testFile);
+    }
+    
+    ULogger::setType(ULogger::kTypeFile, testFile, false);
+    ULogger::setBuffered(true);
+    ULogger::setLevel(ULogger::kInfo);
+    
+    UINFO("Buffered message 1");
+    UINFO("Buffered message 2");
+    
+    // File should be empty before flush
+    EXPECT_EQ(UFile::length(testFile), 0);
+    
+    // Make sure everything is written to file
+    ULogger::setType(ULogger::kTypeNoLog);
+    
+    // File should have content after flush
+    EXPECT_GT(UFile::length(testFile), 0);
+}
+
+TEST(ULoggerTest, GetTime)
+{
+    std::string timeStr;
+    int result = ULogger::getTime(timeStr);
+    
+    EXPECT_GT(result, 0);
+    EXPECT_GT(timeStr.size(), 0u);
+    
+    // Time string should contain date/time format
+    // Format is typically "YYYY-MM-DD HH:MM:SS" or similar
+    EXPECT_NE(timeStr.find("-"), std::string::npos);
+}
+
+TEST(ULoggerTest, SetEventLevel)
+{
+    ULogger::reset();
+
+    ULogger::setEventLevel(ULogger::kDebug);
+    EXPECT_EQ(ULogger::eventLevel(), ULogger::kDebug);
+
+    ULogger::setEventLevel(ULogger::kInfo);
+    EXPECT_EQ(ULogger::eventLevel(), ULogger::kInfo);
+
+    ULogger::setEventLevel(ULogger::kWarning);
+    EXPECT_EQ(ULogger::eventLevel(), ULogger::kWarning);
+    
+    ULogger::setEventLevel(ULogger::kError);
+    EXPECT_EQ(ULogger::eventLevel(), ULogger::kError);
+}
+
+TEST(ULoggerTest, RegisterCurrentThread)
+{
+    ULogger::reset();
+    ULogger::registerCurrentThread("TestThread");
+    
+    std::map<std::string, unsigned long> threads = ULogger::getRegisteredThreads();
+    EXPECT_GT(threads.size(), 0u);
+    EXPECT_NE(threads.find("TestThread"), threads.end());
+    
+    ULogger::unregisterCurrentThread();
+    
+    threads = ULogger::getRegisteredThreads();
+    EXPECT_EQ(threads.find("TestThread"), threads.end());
+}
+
+TEST(ULoggerTest, ThreadIdFilter)
+{
+    ULogger::reset();
+
+    unsigned long currentId = UThread::currentThreadId();
+    std::set<unsigned long> filter;
+    filter.insert(currentId);
+    
+    ULogger::setTreadIdFilter(filter);
+    
+    const std::set<unsigned long>& retrieved = ULogger::getTreadIdFilter();
+    EXPECT_EQ(retrieved.size(), 1u);
+    EXPECT_NE(retrieved.find(currentId), retrieved.end());
+}
+
+class OtherThread : public UThread
+{
+public:
+    OtherThread() : logOnce_(false) {}
+    std::string name() {return "OtherThread";}
+    void logOnce() {logOnce_ = true;}
+protected:
+    virtual void mainLoopBegin() {ULogger::registerCurrentThread(name());}
+    virtual void mainLoopEnd() {ULogger::unregisterCurrentThread();}
+    virtual void mainLoop() {
+        uSleep(10);
+        if(logOnce_)
+        {
+            UINFO("Logging from OtherThread");
+            logOnce_ = false;
+        }
+    }
+private:
+    bool logOnce_;
+};
+
+TEST(ULoggerTest, ThreadIdFilterVector)
+{
+    ULogger::reset();
+
+    // Both threads are registering to logger
+    ULogger::registerCurrentThread("FilterThread");
+    OtherThread otherThread;
+    otherThread.start();
+    
+    std::vector<std::string> threadNames;
+    threadNames.push_back(otherThread.name());
+    threadNames.push_back("FilterThread");
+    
+    ULogger::setTreadIdFilter(threadNames);
+    
+    const std::set<unsigned long>& retrieved = ULogger::getTreadIdFilter();
+    EXPECT_EQ(retrieved.size(), 2u);
+
+    std::string testFile = "logger_thread_log_filtering_test.txt";
+    ULogger::setType(ULogger::kTypeFile, testFile, false);
+    UINFO("This log should appear");
+
+    threadNames.pop_back();
+    ULogger::setTreadIdFilter(threadNames);
+    UINFO("This log should not appear");
+
+    otherThread.logOnce();
+    otherThread.join(true);
+
+    // Make sure the log file is closed
+    ULogger::setType(ULogger::kTypeNoLog);
+
+    std::ifstream file(testFile);
+    std::string line;
+    EXPECT_TRUE(file.is_open());
+
+    std::getline(file, line);
+    EXPECT_TRUE(uStrContains(line, "This log should appear"));
+    std::getline(file, line);
+    EXPECT_TRUE(uStrContains(line, "Logging from OtherThread"));
+    file.close(); 
+    
+    ULogger::unregisterCurrentThread();
+}
+
+TEST(ULoggerTest, WriteWithLevel)
+{
+    ULogger::reset();
+
+    std::string testFile = "logger_write_level_test.txt";
+    ULogger::setType(ULogger::kTypeFile, testFile, false);
+
+    ULogger::setLevel(ULogger::kDebug);
+    
+    // Test write with different levels
+    ULogger::write(ULogger::kDebug, __FILE__, __LINE__, __FUNCTION__, "Debug message %d", 42);
+    ULogger::write(ULogger::kInfo, __FILE__, __LINE__, __FUNCTION__, "Info message %s", "test");
+    ULogger::write(ULogger::kWarning, __FILE__, __LINE__, __FUNCTION__, "Warning message");
+    ULogger::write(ULogger::kError, __FILE__, __LINE__, __FUNCTION__, "Error message");
+
+    // Make sure the log file is closed
+    ULogger::setType(ULogger::kTypeNoLog);
+
+    std::ifstream file(testFile);
+    std::string line;
+    EXPECT_TRUE(file.is_open());
+
+    std::getline(file, line);
+    EXPECT_TRUE(uStrContains(line, "[DEBUG]"));
+    EXPECT_TRUE(uStrContains(line, "Debug message 42"));
+    std::getline(file, line);
+    EXPECT_TRUE(uStrContains(line, "[ INFO]"));
+    EXPECT_TRUE(uStrContains(line, "Info message test"));
+    std::getline(file, line);
+    EXPECT_TRUE(uStrContains(line, "[ WARN]"));
+    EXPECT_TRUE(uStrContains(line, "Warning message"));
+    std::getline(file, line);
+    EXPECT_TRUE(uStrContains(line, "[ERROR]"));
+    EXPECT_TRUE(uStrContains(line, "Error message"));
+    file.close(); 
+}
+
+TEST(ULoggerTest, Macros)
+{
+    ULogger::reset();
+    std::string testFile = "logger_macros_test.txt";
+    ULogger::setType(ULogger::kTypeFile, testFile, false);
+
+    ULogger::setLevel(ULogger::kDebug);
+    
+    UDEBUG("Debug macro message");
+    UINFO("Info macro message");
+    UWARN("Warning macro message");
+    UERROR("Error macro message");
+    
+    // Make sure the log file is closed
+    ULogger::setType(ULogger::kTypeNoLog);
+
+    std::ifstream file(testFile);
+    std::string line;
+    EXPECT_TRUE(file.is_open());
+
+    std::getline(file, line);
+    EXPECT_TRUE(uStrContains(line, "[DEBUG]"));
+    EXPECT_TRUE(uStrContains(line, "Debug macro message"));
+    std::getline(file, line);
+    EXPECT_TRUE(uStrContains(line, "[ INFO]"));
+    EXPECT_TRUE(uStrContains(line, "Info macro message"));
+    std::getline(file, line);
+    EXPECT_TRUE(uStrContains(line, "[ WARN]"));
+    EXPECT_TRUE(uStrContains(line, "Warning macro message"));
+    std::getline(file, line);
+    EXPECT_TRUE(uStrContains(line, "[ERROR]"));
+    EXPECT_TRUE(uStrContains(line, "Error macro message"));
+    file.close(); 
+}
+
+TEST(ULoggerTest, Reset)
+{
+    ULogger::reset();
+    // Change some settings
+    ULogger::setType(ULogger::kTypeConsole);
+    ULogger::setLevel(ULogger::kDebug);
+    ULogger::setPrintTime(false);
+    ULogger::setPrintLevel(false);
+    ULogger::setBuffered(true);
+    
+    // Reset
+    ULogger::reset();
+    
+    // Should be back to defaults
+    EXPECT_EQ(ULogger::type(), ULogger::kTypeNoLog);
+    EXPECT_EQ(ULogger::level(), ULogger::kInfo);
+    EXPECT_TRUE(ULogger::isPrintTime());
+    EXPECT_TRUE(ULogger::isPrintLevel());
+    EXPECT_FALSE(ULogger::isBuffered());
+}
+
+TEST(ULoggerTest, MultipleLogLevels)
+{
+    ULogger::reset();
+    std::string testFile = "logger_multiple_levels_test.txt";
+    ULogger::setType(ULogger::kTypeFile, testFile, false);
+    
+    // Test all levels
+    ULogger::setLevel(ULogger::kDebug);
+    UDEBUG("Debug");
+    UINFO("Info");
+    UWARN("Warning");
+    UERROR("Error");
+    
+    ULogger::setLevel(ULogger::kInfo);
+    UDEBUG("Debug - should not appear");
+    UINFO("Info");
+    UWARN("Warning");
+    UERROR("Error");
+    
+    ULogger::setLevel(ULogger::kWarning);
+    UDEBUG("Debug - should not appear");
+    UINFO("Info - should not appear");
+    UWARN("Warning");
+    UERROR("Error");
+
+    ULogger::setLevel(ULogger::kError);
+    UDEBUG("Debug - should not appear");
+    UINFO("Info - should not appear");
+    UWARN("Warning - should not appear");
+    UERROR("Error");
+    
+    // Make sure the log file is closed
+    ULogger::setType(ULogger::kTypeNoLog);
+
+    std::ifstream file(testFile);
+    std::string line;
+    EXPECT_TRUE(file.is_open());
+
+    int count = 0;
+    std::getline(file, line);
+    while(!line.empty())
+    {
+        EXPECT_TRUE(!uStrContains(line, "should not appear"));
+        ++count;
+        std::getline(file, line);
+    }
+    EXPECT_EQ(count, 10);
+    file.close(); 
+}
+
+TEST(ULoggerTest, FileContent)
+{
+    ULogger::reset();
+    std::string testFile = "test_ulogger_content.txt";
+    
+    ULogger::setType(ULogger::kTypeFile, testFile, false);
+    ULogger::setLevel(ULogger::kInfo);
+    ULogger::setPrintTime(false);
+    ULogger::setPrintLevel(true);
+    ULogger::setPrintWhere(false);
+    
+    UINFO("Test content message");
+    ULogger::setType(ULogger::kTypeNoLog);
+    
+    // Read file and check content
+    std::ifstream file(testFile);
+    std::string content((std::istreambuf_iterator<char>(file)),
+                        std::istreambuf_iterator<char>());
+    file.close();
+    
+    EXPECT_NE(content.find("Test content message"), std::string::npos);
+}
+
+TEST(ULoggerTest, ThreadSafety)
+{
+    ULogger::reset();
+    std::string testFile = "logger_thread_safety_test.txt";
+    ULogger::setType(ULogger::kTypeFile, testFile, false);
+
+    // Disable all printing to easyly test results below
+    ULogger::setPrintTime(false);
+    ULogger::setPrintLevel(false);
+    ULogger::setPrintWhere(false);
+    ULogger::setPrintWhereFullPath(false);
+    ULogger::setPrintColored(false);
+    ULogger::setPrintThreadId(false);
+    
+    const int numThreads = 5;
+    const int messagesPerThread = 9;
+    
+    std::vector<std::thread> threads;
+    
+    for(int i = 0; i < numThreads; ++i)
+    {
+        threads.emplace_back([i, messagesPerThread]() {
+            for(int j = 0; j < messagesPerThread; ++j)
+            {
+                UINFO("Thread %d message %d", i, j);
+            }
+        });
+    }
+    
+    for(auto& t : threads)
+    {
+        t.join();
+    }
+
+    // Make sure the log file is closed
+    ULogger::setType(ULogger::kTypeNoLog);
+
+    std::ifstream file(testFile);
+    std::string line;
+    EXPECT_TRUE(file.is_open());
+
+    int count = 0;
+    std::getline(file, line);
+    while(!line.empty())
+    {
+        // Expect all messages on single line, no interleaved, format: "Thread X message X"
+        EXPECT_EQ(line.length(), 19);
+        ++count;
+        std::getline(file, line);
+    }
+    EXPECT_EQ(count, 45);
+    file.close(); 
+}
+
+TEST(ULoggerTest, DefaultLogFileName)
+{
+    EXPECT_EQ(ULogger::kDefaultLogFileName, "./ULog.txt");
+}
+
+TEST(ULoggerTest, AllPrintOptions)
+{
+    ULogger::reset();
+    std::string testFile = "logger_all_print_options_test.txt";
+    ULogger::setType(ULogger::kTypeFile, testFile, false);
+    
+    // Test with all print options enabled
+    ULogger::setPrintTime(true);
+    ULogger::setPrintLevel(true);
+    ULogger::setPrintWhereFullPath(true);
+    ULogger::setPrintColored(true);
+    ULogger::setPrintThreadId(true);
+    
+    UINFO("Message with all options");
+    
+    // Test with all print options disabled
+    ULogger::setPrintTime(false);
+    ULogger::setPrintLevel(false);
+    ULogger::setPrintWhere(false);
+    ULogger::setPrintWhereFullPath(false);
+    ULogger::setPrintColored(false);
+    ULogger::setPrintThreadId(false);
+    
+    UINFO("Message with no options");
+    
+    // Make sure the log file is closed
+    ULogger::setType(ULogger::kTypeNoLog);
+
+    std::ifstream file(testFile);
+    std::string line;
+    EXPECT_TRUE(file.is_open());
+
+    std::getline(file, line);
+    EXPECT_TRUE(uStrContains(line, "["));
+    EXPECT_TRUE(uStrContains(line, "{"));
+    EXPECT_TRUE(uStrContains(line, "-"));
+    EXPECT_TRUE(uStrContains(line, ".cpp"));
+    EXPECT_TRUE(uStrContains(line, "/") || uStrContains(line, "\\"));
+   
+    std::getline(file, line);
+    EXPECT_FALSE(uStrContains(line, "["));
+    EXPECT_FALSE(uStrContains(line, "{"));
+    EXPECT_FALSE(uStrContains(line, "-"));
+    EXPECT_FALSE(uStrContains(line, ".cpp"));
+    EXPECT_FALSE(uStrContains(line, "\\"));
+    EXPECT_FALSE(uStrContains(line, "/"));
+
+    file.close(); 
+}
+

utilite/test/test_umath.cpp

@@ -0,0 +1,284 @@
+#include "gtest/gtest.h"
+#include "rtabmap/utilite/UMath.h"
+#include <cmath>
+#include <limits>
+
+TEST(UMathTest, uIsNan)
+{
+    float nanValue = std::numeric_limits<float>::quiet_NaN();
+    float normalValue = 3.14f;
+    
+    EXPECT_TRUE(uIsNan(nanValue));
+    EXPECT_FALSE(uIsNan(normalValue));
+    
+    double nanDouble = std::numeric_limits<double>::quiet_NaN();
+    double normalDouble = 3.14;
+    EXPECT_TRUE(uIsNan(nanDouble));
+    EXPECT_FALSE(uIsNan(normalDouble));
+}
+
+TEST(UMathTest, uIsFinite)
+{
+    float infValue = std::numeric_limits<float>::infinity();
+    float normalValue = 3.14f;
+    float nanValue = std::numeric_limits<float>::quiet_NaN();
+    
+    EXPECT_FALSE(uIsFinite(infValue));
+    EXPECT_TRUE(uIsFinite(normalValue));
+    EXPECT_FALSE(uIsFinite(nanValue));
+}
+
+TEST(UMathTest, uMin3)
+{
+    EXPECT_EQ(uMin3(3, 1, 2), 1);
+    EXPECT_EQ(uMin3(1, 2, 3), 1);
+    EXPECT_EQ(uMin3(2, 3, 1), 1);
+    EXPECT_EQ(uMin3(1.5f, 2.5f, 0.5f), 0.5f);
+}
+
+TEST(UMathTest, uMax3)
+{
+    EXPECT_EQ(uMax3(3, 1, 2), 3);
+    EXPECT_EQ(uMax3(1, 2, 3), 3);
+    EXPECT_EQ(uMax3(2, 3, 1), 3);
+    EXPECT_EQ(uMax3(1.5f, 2.5f, 0.5f), 2.5f);
+}
+
+TEST(UMathTest, uMaxArray)
+{
+    int arr[] = {1, 5, 3, 9, 2};
+    unsigned int index;
+    int max = uMax(arr, 5, index);
+    EXPECT_EQ(max, 9);
+    EXPECT_EQ(index, 3u);
+    
+    int max2 = uMax(arr, 5);
+    EXPECT_EQ(max2, 9);
+}
+
+TEST(UMathTest, uMaxVector)
+{
+    std::vector<int> v = {1, 5, 3, 9, 2};
+    unsigned int index;
+    int max = uMax(v, index);
+    EXPECT_EQ(max, 9);
+    EXPECT_EQ(index, 3u);
+    
+    int max2 = uMax(v);
+    EXPECT_EQ(max2, 9);
+}
+
+TEST(UMathTest, uMinArray)
+{
+    int arr[] = {5, 1, 3, 9, 2};
+    unsigned int index;
+    int min = uMin(arr, 5, index);
+    EXPECT_EQ(min, 1);
+    EXPECT_EQ(index, 1u);
+    
+    int min2 = uMin(arr, 5);
+    EXPECT_EQ(min2, 1);
+}
+
+TEST(UMathTest, uMinVector)
+{
+    std::vector<int> v = {5, 1, 3, 9, 2};
+    unsigned int index;
+    int min = uMin(v, index);
+    EXPECT_EQ(min, 1);
+    EXPECT_EQ(index, 1u);
+    
+    int min2 = uMin(v);
+    EXPECT_EQ(min2, 1);
+}
+
+TEST(UMathTest, uMinMaxArray)
+{
+    int arr[] = {5, 1, 3, 9, 2};
+    int min, max;
+    unsigned int indexMin, indexMax;
+    uMinMax(arr, 5, min, max, indexMin, indexMax);
+    EXPECT_EQ(min, 1);
+    EXPECT_EQ(max, 9);
+    EXPECT_EQ(indexMin, 1u);
+    EXPECT_EQ(indexMax, 3u);
+}
+
+TEST(UMathTest, uMinMaxVector)
+{
+    std::vector<int> v = {5, 1, 3, 9, 2};
+    int min, max;
+    unsigned int indexMin, indexMax;
+    uMinMax(v, min, max, indexMin, indexMax);
+    EXPECT_EQ(min, 1);
+    EXPECT_EQ(max, 9);
+    EXPECT_EQ(indexMin, 1u);
+    EXPECT_EQ(indexMax, 3u);
+}
+
+TEST(UMathTest, uMinMaxWithoutIndices)
+{
+    int arr[] = {5, 1, 3, 9, 2};
+    int min, max;
+    uMinMax(arr, 5, min, max);
+    EXPECT_EQ(min, 1);
+    EXPECT_EQ(max, 9);
+    
+    std::vector<int> v = {5, 1, 3, 9, 2};
+    uMinMax(v, min, max);
+    EXPECT_EQ(min, 1);
+    EXPECT_EQ(max, 9);
+}
+
+TEST(UMathTest, uSign)
+{
+    EXPECT_EQ(uSign(5), 1);
+    EXPECT_EQ(uSign(-5), -1);
+    EXPECT_EQ(uSign(0), 1);
+    EXPECT_EQ(uSign(3.14f), 1);
+    EXPECT_EQ(uSign(-3.14f), -1);
+}
+
+TEST(UMathTest, uSumList)
+{
+    std::list<int> list = {1, 2, 3, 4, 5};
+    int sum = uSum(list);
+    EXPECT_EQ(sum, 15);
+}
+
+TEST(UMathTest, uSumArray)
+{
+    int arr[] = {1, 2, 3, 4, 5};
+    int sum = uSum(arr, 5);
+    EXPECT_EQ(sum, 15);
+}
+
+TEST(UMathTest, uSumVector)
+{
+    std::vector<int> v = {1, 2, 3, 4, 5};
+    int sum = uSum(v);
+    EXPECT_EQ(sum, 15);
+}
+
+TEST(UMathTest, uSumSquared)
+{
+    int arr[] = {1, 2, 3};
+    int sum = uSumSquared(arr, 3);
+    EXPECT_EQ(sum, 14); // 1*1 + 2*2 + 3*3 = 1 + 4 + 9 = 14
+    
+    int sum2 = uSumSquared(arr, 3, 2); // subtract 2
+    EXPECT_EQ(sum2, 2); // (-1)*(-1) + 0*0 + 1*1 = 1 + 0 + 1 = 2
+}
+
+TEST(UMathTest, uMean)
+{
+    int arr[] = {1, 2, 3, 4, 5};
+    float mean = uMean(arr, 5);
+    EXPECT_FLOAT_EQ(mean, 3.0f);
+    
+    std::vector<int> v = {1, 2, 3, 4, 5};
+    float mean2 = uMean(v);
+    EXPECT_FLOAT_EQ(mean2, 3.0f);
+    
+    std::list<int> list = {1, 2, 3, 4, 5};
+    float mean3 = uMean(list);
+    EXPECT_FLOAT_EQ(mean3, 3.0f);
+}
+
+TEST(UMathTest, uMeanSquaredError)
+{
+    float x[] = {1.0f, 2.0f, 3.0f};
+    float y[] = {1.1f, 2.1f, 3.1f};
+    float mse = uMeanSquaredError(x, 3, y, 3);
+    EXPECT_NEAR(mse, 0.01f, 0.001f); // (0.1^2 + 0.1^2 + 0.1^2) / 3 = 0.01
+    
+    std::vector<float> vx = {1.0f, 2.0f, 3.0f};
+    std::vector<float> vy = {1.1f, 2.1f, 3.1f};
+    float mse2 = uMeanSquaredError(vx, vy);
+    EXPECT_NEAR(mse2, 0.01f, 0.001f);
+}
+
+TEST(UMathTest, uVariance)
+{
+    float arr[] = {1.0f, 2.0f, 3.0f, 4.0f, 5.0f};
+    float mean = uMean(arr, 5);
+    float variance = uVariance(arr, 5, mean);
+    EXPECT_NEAR(variance, 2.5f, 0.1f); // Variance of 1,2,3,4,5 is 2.5
+    
+    float variance2 = uVariance(arr, 5);
+    EXPECT_NEAR(variance2, 2.5f, 0.1f);
+}
+
+TEST(UMathTest, uNormSquared)
+{
+    std::vector<float> v = {3.0f, 4.0f};
+    float normSq = uNormSquared(v);
+    EXPECT_FLOAT_EQ(normSq, 25.0f); // 3*3 + 4*4 = 9 + 16 = 25
+    
+    float normSq2 = uNormSquared(3.0f, 4.0f);
+    EXPECT_FLOAT_EQ(normSq2, 25.0f);
+    
+    float normSq3 = uNormSquared(1.0f, 2.0f, 3.0f);
+    EXPECT_FLOAT_EQ(normSq3, 14.0f); // 1*1 + 2*2 + 3*3 = 1 + 4 + 9 = 14
+}
+
+TEST(UMathTest, uNorm)
+{
+    std::vector<float> v = {3.0f, 4.0f};
+    float norm = uNorm(v);
+    EXPECT_FLOAT_EQ(norm, 5.0f); // sqrt(3*3 + 4*4) = sqrt(25) = 5
+    
+    float norm2 = uNorm(3.0f, 4.0f);
+    EXPECT_FLOAT_EQ(norm2, 5.0f);
+    
+    float norm3 = uNorm(1.0f, 2.0f, 3.0f);
+    EXPECT_NEAR(norm3, 3.741657f, 0.001f); // sqrt(14)
+}
+
+TEST(UMathTest, uNormalize)
+{
+    std::vector<float> v = {3.0f, 4.0f};
+    std::vector<float> normalized = uNormalize(v);
+    float norm = uNorm(normalized);
+    EXPECT_NEAR(norm, 1.0f, 0.001f);
+    
+    std::vector<float> zero = {0.0f, 0.0f};
+    std::vector<float> normalizedZero = uNormalize(zero);
+    EXPECT_EQ(normalizedZero.size(), 2u);
+    EXPECT_FLOAT_EQ(normalizedZero[0], 0.0f);
+    EXPECT_FLOAT_EQ(normalizedZero[1], 0.0f);
+}
+
+TEST(UMathTest, uLocalMaxima)
+{
+    float arr[] = {1.0f, 3.0f, 2.0f, 5.0f, 4.0f, 6.0f, 3.0f};
+    std::list<unsigned int> maxima = uLocalMaxima(arr, 7);
+    // Should find maxima at indices 1, 3, 5
+    EXPECT_GT(maxima.size(), 0u);
+    
+    std::vector<float> v = {1.0f, 3.0f, 2.0f, 5.0f, 4.0f};
+    std::list<unsigned int> maxima2 = uLocalMaxima(v);
+    EXPECT_GT(maxima2.size(), 0u);
+}
+
+TEST(UMathTest, uHamming)
+{
+    std::vector<float> window = uHamming(10);
+    EXPECT_EQ(window.size(), 10u);
+    // Hamming window values should be between 0 and 1
+    for(float w : window)
+    {
+        EXPECT_GE(w, 0.0f);
+        EXPECT_LE(w, 1.0f);
+    }
+}
+
+TEST(UMathTest, uIsInBounds)
+{
+    EXPECT_TRUE(uIsInBounds(5, 0, 10));
+    EXPECT_FALSE(uIsInBounds(10, 0, 10));
+    EXPECT_FALSE(uIsInBounds(-1, 0, 10));
+    EXPECT_FALSE(uIsInBounds(std::numeric_limits<float>::quiet_NaN(), 0.0f, 10.0f));
+    EXPECT_FALSE(uIsInBounds(std::numeric_limits<float>::infinity(), 0.0f, 10.0f));
+}
+

utilite/test/test_umutex.cpp

@@ -0,0 +1,172 @@
+#include "gtest/gtest.h"
+#include "rtabmap/utilite/UMutex.h"
+#include <thread>
+#include <chrono>
+#include <atomic>
+#include <vector>
+
+TEST(UMutexTest, Constructor)
+{
+    UMutex mutex;
+    // Should be constructible
+    SUCCEED();
+}
+
+TEST(UMutexTest, LockUnlock)
+{
+    UMutex mutex;
+    EXPECT_TRUE(mutex.lock());
+    EXPECT_TRUE(mutex.unlock());
+}
+
+TEST(UMutexTest, LockTry)
+{
+    UMutex mutex;
+    
+    // Try to lock when not locked
+    EXPECT_TRUE(mutex.lockTry());
+    
+    // Unlock
+    mutex.unlock();
+}
+
+TEST(UMutexTest, LockTryWhenLocked)
+{
+    UMutex mutex;
+    
+    // Lock the mutex
+    mutex.lock();
+    
+    // Try to lock from another thread
+    std::atomic<bool> tryResult(false);
+    std::thread t([&mutex, &tryResult]() {
+        tryResult = mutex.lockTry(); // Should fail (busy or similar)
+    });
+    
+    t.join();
+    
+    EXPECT_FALSE(tryResult);
+    
+    mutex.unlock();
+}
+
+TEST(UMutexTest, ThreadSafety)
+{
+    UMutex mutex;
+    std::atomic<int> counter(0);
+    const int numThreads = 4;
+    const int iterations = 1000;
+    
+    std::vector<std::thread> threads;
+    
+    for(int i = 0; i < numThreads; ++i)
+    {
+        threads.emplace_back([&mutex, &counter, iterations]() {
+            for(int j = 0; j < iterations; ++j)
+            {
+                mutex.lock();
+                int val = counter.load();
+                std::this_thread::sleep_for(std::chrono::microseconds(1));
+                counter.store(val + 1);
+                mutex.unlock();
+            }
+        });
+    }
+    
+    for(auto& t : threads)
+    {
+        t.join();
+    }
+    
+    EXPECT_EQ(counter.load(), numThreads * iterations);
+}
+
+TEST(UMutexTest, RecursiveLock)
+{
+    UMutex mutex;
+    
+    // Lock multiple times from same thread (should work on Unix)
+    EXPECT_TRUE(mutex.lock());
+    EXPECT_TRUE(mutex.lock());
+    EXPECT_TRUE(mutex.lock());
+    
+    // Unlock multiple times
+    EXPECT_TRUE(mutex.unlock());
+    EXPECT_TRUE(mutex.unlock());
+    EXPECT_TRUE(mutex.unlock());
+}
+
+TEST(UMutexTest, UScopeMutex)
+{
+    UMutex mutex;  
+    {
+        UScopeMutex scopeMutex(mutex);
+        // Mutex should be locked here
+
+        // Try to lock from another thread
+        std::thread t([&mutex]() {
+            EXPECT_FALSE(mutex.lockTry()); // Should fail
+        });
+        t.join();
+    }
+    // Mutex should be unlocked here
+    
+    // Try to lock from another thread
+    std::thread t([&mutex]() {
+        EXPECT_TRUE(mutex.lockTry()); // Should succeed
+        mutex.unlock();
+    });
+    t.join();
+}
+
+TEST(UMutexTest, UScopeMutexWithPointer)
+{
+    UMutex mutex;
+    
+    {
+        UScopeMutex scopeMutex(&mutex);
+        // Mutex should be locked here
+
+        // Try to lock from another thread
+        std::thread t([&mutex]() {
+            EXPECT_FALSE(mutex.lockTry()); // Should fail
+        });
+        t.join();
+    }
+    // Mutex should be unlocked here
+    
+    // Try to lock from another thread
+    std::thread t([&mutex]() {
+        EXPECT_TRUE(mutex.lockTry()); // Should succeed
+        mutex.unlock();
+    });
+    t.join();
+}
+
+TEST(UMutexTest, MultipleMutexes)
+{
+    UMutex mutex1;
+    UMutex mutex2;
+    
+    mutex1.lock();
+    mutex2.lock();
+    
+    mutex1.unlock();
+    mutex2.unlock();
+    
+    SUCCEED();
+}
+
+TEST(UMutexTest, LockUnlockSequence)
+{
+    UMutex mutex;
+    
+    for(int i = 0; i < 10; ++i)
+    {
+        mutex.lock();
+        mutex.unlock();
+    }
+    
+    SUCCEED();
+}
+

utilite/test/test_uprocessinfo.cpp

@@ -0,0 +1,30 @@
+#include "gtest/gtest.h"
+#include "rtabmap/utilite/UProcessInfo.h"
+
+TEST(UProcessInfoTest, Constructor)
+{
+    UProcessInfo info;
+    // Should be constructible
+    SUCCEED();
+}
+
+TEST(UProcessInfoTest, GetMemoryUsage)
+{
+    long int memory = UProcessInfo::getMemoryUsage();
+    // Memory usage should be positive
+    EXPECT_GT(memory, 0);
+    // Should be reasonable (less than 1TB)
+    EXPECT_LT(memory, 1024LL * 1024 * 1024 * 1024);
+}
+
+TEST(UProcessInfoTest, GetMemoryUsageMultipleCalls)
+{
+    long int memory1 = UProcessInfo::getMemoryUsage();
+    long int memory2 = UProcessInfo::getMemoryUsage();
+    // Both calls should return valid values
+    EXPECT_GT(memory1, 0);
+    EXPECT_GT(memory2, 0);
+    // Values should be similar (might vary slightly)
+    EXPECT_NEAR(memory1, memory2, memory1 * 0.1); // Allow 10% variation
+}
+

utilite/test/test_usemaphore.cpp

@@ -0,0 +1,333 @@
+#include "gtest/gtest.h"
+#include "rtabmap/utilite/USemaphore.h"
+#include "rtabmap/utilite/UMutex.h"
+#include "rtabmap/utilite/UTimer.h"
+#include <thread>
+#include <chrono>
+#include <atomic>
+#include <vector>
+
+TEST(USemaphoreTest, ConstructorDefault)
+{
+    USemaphore sem;
+    // Should be constructible with default value (0)
+    EXPECT_EQ(sem.value(), 0);
+}
+
+TEST(USemaphoreTest, ConstructorWithValue)
+{
+    USemaphore sem(5);
+    EXPECT_EQ(sem.value(), 5);
+}
+
+TEST(USemaphoreTest, Release)
+{
+    USemaphore sem(0);
+    
+    sem.release();
+    EXPECT_EQ(sem.value(), 1);
+    
+    sem.release(3);
+    EXPECT_EQ(sem.value(), 4);
+}
+
+TEST(USemaphoreTest, Acquire)
+{
+    USemaphore sem(2);
+    
+    bool result = sem.acquire();
+    EXPECT_TRUE(result);
+    EXPECT_EQ(sem.value(), 1);
+    
+    result = sem.acquire();
+    EXPECT_TRUE(result);
+    EXPECT_EQ(sem.value(), 0);
+}
+
+TEST(USemaphoreTest, AcquireMultiple)
+{
+    USemaphore sem(5);
+    
+    bool result = sem.acquire(3);
+    EXPECT_TRUE(result);
+    EXPECT_EQ(sem.value(), 2);
+    
+    result = sem.acquire(2);
+    EXPECT_TRUE(result);
+    EXPECT_EQ(sem.value(), 0);
+}
+
+TEST(USemaphoreTest, AcquireBlocks)
+{
+    USemaphore sem(0);
+    std::atomic<bool> acquired(false);
+    std::atomic<bool> threadStarted(false);
+    
+    std::thread t([&sem, &acquired, &threadStarted]() {
+        threadStarted = true;
+        bool result = sem.acquire();
+        acquired = result;
+    });
+    
+    // Wait for thread to start and try to acquire
+    while(!threadStarted)
+    {
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    
+    // Thread should be blocked
+    EXPECT_FALSE(acquired);
+    
+    // Release to unblock
+    sem.release();
+    
+    t.join();
+    
+    EXPECT_TRUE(acquired);
+}
+
+TEST(USemaphoreTest, AcquireWithTimeout)
+{
+    USemaphore sem(0);
+    
+    // Try to acquire with timeout
+    bool result = sem.acquire(1, 50); // 50ms timeout
+    EXPECT_FALSE(result); // Should timeout
+    
+    // Release and try again
+    sem.release();
+    result = sem.acquire(1, 50);
+    EXPECT_TRUE(result);
+}
+
+TEST(USemaphoreTest, AcquireTry)
+{
+    USemaphore sem(3);
+    
+    // Try to acquire when available
+#ifdef _WIN32
+    bool result = sem.acquireTry();
+    EXPECT_TRUE(result);
+    EXPECT_EQ(sem.value(), 2);
+    result = sem.acquireTry();
+    EXPECT_TRUE(result);
+    EXPECT_EQ(sem.value(), 1);
+    result = sem.acquireTry();
+    EXPECT_TRUE(result);
+    EXPECT_EQ(sem.value(), 0);
+    result = sem.acquireTry();
+    EXPECT_TRUE(result);
+#else
+    int result = sem.acquireTry(1);
+    EXPECT_TRUE(result);
+    EXPECT_EQ(sem.value(), 2);
+    
+    result = sem.acquireTry(2);
+    EXPECT_TRUE(result);
+    EXPECT_EQ(sem.value(), 0);
+    
+    // Try to acquire when not available
+    result = sem.acquireTry(1);
+    EXPECT_FALSE(result);
+#endif
+}
+
+TEST(USemaphoreTest, Value)
+{
+    USemaphore sem(10);
+    
+    int value = sem.value();
+    EXPECT_EQ(value, 10);
+    
+    sem.acquire(3);
+    value = sem.value();
+    EXPECT_EQ(value, 7);
+    
+    sem.release(2);
+    value = sem.value();
+    EXPECT_EQ(value, 9);
+}
+
+TEST(USemaphoreTest, ThreadSafety)
+{
+    USemaphore sem(0);
+    std::atomic<int> counter(0);
+    const int numThreads = 4;
+    const int iterations = 100;
+    
+    std::vector<std::thread> threads;
+    
+    // Create threads that will wait on semaphore
+    for(int i = 0; i < numThreads; ++i)
+    {
+        threads.emplace_back([&sem, &counter, iterations]() {
+            for(int j = 0; j < iterations; ++j)
+            {
+                sem.acquire();
+                counter++;
+            }
+        });
+    }
+    
+    // Release semaphore to allow threads to proceed
+    std::thread releaseThread([&sem, numThreads, iterations]() {
+        for(int i = 0; i < numThreads * iterations; ++i)
+        {
+            sem.release();
+            std::this_thread::sleep_for(std::chrono::microseconds(10));
+        }
+    });
+    
+    for(auto& t : threads)
+    {
+        t.join();
+    }
+    
+    releaseThread.join();
+    
+    EXPECT_EQ(counter.load(), numThreads * iterations);
+}
+
+TEST(USemaphoreTest, ProducerConsumer)
+{
+    USemaphore full(0);
+    USemaphore empty(10);
+    std::vector<int> buffer;
+    UMutex bufferMutex;
+    const int items = 20;
+    std::atomic<int> produced(0);
+    std::atomic<int> consumed(0);
+    
+    // Producer thread
+    std::thread producer([&full, &empty, &buffer, &bufferMutex, &produced, items]() {
+        for(int i = 0; i < items; ++i)
+        {
+            empty.acquire();
+            {
+                UScopeMutex lock(bufferMutex);
+                buffer.push_back(i);
+            }
+            full.release();
+            produced++;
+        }
+    });
+    
+    // Consumer thread
+    std::thread consumer([&full, &empty, &buffer, &bufferMutex, &consumed, items]() {
+        for(int i = 0; i < items; ++i)
+        {
+            full.acquire();
+            {
+                UScopeMutex lock(bufferMutex);
+                if(!buffer.empty())
+                {
+                    buffer.pop_back();
+                }
+            }
+            empty.release();
+            consumed++;
+        }
+    });
+    
+    producer.join();
+    consumer.join();
+    
+    EXPECT_EQ(produced.load(), items);
+    EXPECT_EQ(consumed.load(), items);
+}
+
+TEST(USemaphoreTest, MultipleAcquireRelease)
+{
+    USemaphore sem(0);
+    
+    // Release multiple times
+    sem.release(5);
+    EXPECT_EQ(sem.value(), 5);
+    
+    // Acquire multiple times
+    bool result = sem.acquire(3);
+    EXPECT_TRUE(result);
+    EXPECT_EQ(sem.value(), 2);
+    
+    result = sem.acquire(2);
+    EXPECT_TRUE(result);
+    EXPECT_EQ(sem.value(), 0);
+}
+
+TEST(USemaphoreTest, AcquireZero)
+{
+    USemaphore sem(5);
+    
+    // Acquire 0 should not block
+    bool result = sem.acquire(0);
+    EXPECT_TRUE(result);
+    EXPECT_EQ(sem.value(), 5);
+}
+
+TEST(USemaphoreTest, ReleaseZero)
+{
+    USemaphore sem(5);
+    
+    // Release 0 should not change value
+    sem.release(0);
+    EXPECT_EQ(sem.value(), 5);
+}
+
+#ifdef _WIN32
+TEST(USemaphoreTest, Reset)
+{
+    USemaphore sem(5);
+    EXPECT_EQ(sem.value(), 5);
+    
+    sem.reset(10);
+    EXPECT_EQ(sem.value(), 10);
+    
+    sem.reset(0);
+    EXPECT_EQ(sem.value(), 0);
+}
+#endif
+
+TEST(USemaphoreTest, ConcurrentAcquireRelease)
+{
+    USemaphore sem(0);
+    std::atomic<int> acquiredCount(0);
+    const int numThreads = 5;
+    
+    std::vector<std::thread> acquireThreads;
+    std::vector<std::thread> releaseThreads;
+    
+    // Create threads that acquire
+    for(int i = 0; i < numThreads; ++i)
+    {
+        acquireThreads.emplace_back([&sem, &acquiredCount]() {
+            sem.acquire();
+            acquiredCount++;
+        });
+    }
+    
+    // Wait a bit
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    
+    // Create threads that release
+    for(int i = 0; i < numThreads; ++i)
+    {
+        releaseThreads.emplace_back([&sem]() {
+            std::this_thread::sleep_for(std::chrono::milliseconds(10));
+            sem.release();
+        });
+    }
+    
+    for(auto& t : acquireThreads)
+    {
+        t.join();
+    }
+    
+    for(auto& t : releaseThreads)
+    {
+        t.join();
+    }
+    
+    EXPECT_EQ(acquiredCount.load(), numThreads);
+}
+

utilite/test/test_ustl.cpp

@@ -0,0 +1,347 @@
+#include "gtest/gtest.h"
+#include "rtabmap/utilite/UStl.h"
+#include <map>
+
+TEST(UStlTest, uUniqueKeys)
+{
+    std::multimap<int, std::string> mm;
+    mm.insert({1, "a"});
+    mm.insert({1, "b"});
+    mm.insert({2, "c"});
+    mm.insert({3, "d"});
+    mm.insert({3, "e"});
+    
+    std::list<int> uniqueKeys = uUniqueKeys(mm);
+    EXPECT_EQ(uniqueKeys.size(), 3u);
+    EXPECT_EQ(uniqueKeys.front(), 1);
+    EXPECT_EQ(uniqueKeys.back(), 3);
+}
+
+TEST(UStlTest, uKeysMultimap)
+{
+    std::multimap<int, std::string> mm;
+    mm.insert({1, "a"});
+    mm.insert({2, "b"});
+    mm.insert({3, "c"});
+    
+    std::vector<int> keys = uKeys(mm);
+    EXPECT_EQ(keys.size(), 3u);
+    
+    std::list<int> keysList = uKeysList(mm);
+    EXPECT_EQ(keysList.size(), 3u);
+}
+
+TEST(UStlTest, uValuesMultimap)
+{
+    std::multimap<int, std::string> mm;
+    mm.insert({1, "a"});
+    mm.insert({1, "b"});
+    mm.insert({2, "c"});
+    
+    std::vector<std::string> values = uValues(mm);
+    EXPECT_EQ(values.size(), 3u);
+    
+    std::list<std::string> valuesList = uValuesList(mm);
+    EXPECT_EQ(valuesList.size(), 3u);
+}
+
+TEST(UStlTest, uValuesMultimapForKey)
+{
+    std::multimap<int, std::string> mm;
+    mm.insert({1, "a"});
+    mm.insert({1, "b"});
+    mm.insert({2, "c"});
+    
+    std::list<std::string> values = uValues(mm, 1);
+    EXPECT_EQ(values.size(), 2u);
+    EXPECT_EQ(values.front(), "a");
+    EXPECT_EQ(values.back(), "b");
+}
+
+TEST(UStlTest, uKeysMap)
+{
+    std::map<int, std::string> m;
+    m[1] = "a";
+    m[2] = "b";
+    m[3] = "c";
+    
+    std::vector<int> keys = uKeys(m);
+    EXPECT_EQ(keys.size(), 3u);
+    
+    std::list<int> keysList = uKeysList(m);
+    EXPECT_EQ(keysList.size(), 3u);
+    
+    std::set<int> keysSet = uKeysSet(m);
+    EXPECT_EQ(keysSet.size(), 3u);
+}
+
+TEST(UStlTest, uValuesMap)
+{
+    std::map<int, std::string> m;
+    m[1] = "a";
+    m[2] = "b";
+    m[3] = "c";
+    
+    std::vector<std::string> values = uValues(m);
+    EXPECT_EQ(values.size(), 3u);
+    
+    std::list<std::string> valuesList = uValuesList(m);
+    EXPECT_EQ(valuesList.size(), 3u);
+}
+
+TEST(UStlTest, uValueMap)
+{
+    std::map<int, std::string> m;
+    m[1] = "a";
+    m[2] = "b";
+    
+    std::string value = uValue(m, 1, std::string("default"));
+    EXPECT_EQ(value, "a");
+    
+    std::string defaultValue = uValue(m, 99, std::string("default"));
+    EXPECT_EQ(defaultValue, "default");
+}
+
+TEST(UStlTest, uTake)
+{
+    std::map<int, std::string> m;
+    m[1] = "a";
+    m[2] = "b";
+    
+    std::string value = uTake(m, 1, std::string("default"));
+    EXPECT_EQ(value, "a");
+    EXPECT_EQ(m.size(), 1u);
+    EXPECT_EQ(m.find(1), m.end());
+}
+
+TEST(UStlTest, uIteratorAtList)
+{
+    std::list<int> list = {1, 2, 3, 4, 5};
+    auto iter = uIteratorAt(list, 2);
+    EXPECT_EQ(*iter, 3);
+    
+    const std::list<int>& constList = list;
+    auto constIter = uIteratorAt(constList, 2);
+    EXPECT_EQ(*constIter, 3);
+}
+
+TEST(UStlTest, uIteratorAtSet)
+{
+    std::set<int> s = {1, 2, 3, 4, 5};
+    auto iter = uIteratorAt(s, 2);
+    EXPECT_NE(iter, s.end());
+}
+
+TEST(UStlTest, uIteratorAtVector)
+{
+    std::vector<int> v = {1, 2, 3, 4, 5};
+    auto iter = uIteratorAt(v, 2);
+    EXPECT_EQ(*iter, 3);
+    
+    const std::vector<int>& constV = v;
+    auto constIter = uIteratorAt(constV, 2);
+    EXPECT_EQ(*constIter, 3);
+}
+
+TEST(UStlTest, uValueAtList)
+{
+    std::list<int> list = {1, 2, 3, 4, 5};
+    int& value = uValueAt(list, 2);
+    EXPECT_EQ(value, 3);
+    value = 10;
+    EXPECT_EQ(uValueAt(list, 2), 10);
+    
+    const std::list<int>& constList = list;
+    const int& constValue = uValueAt(constList, 2);
+    EXPECT_EQ(constValue, 10);
+}
+
+TEST(UStlTest, uContainsList)
+{
+    std::list<int> list = {1, 2, 3, 4, 5};
+    EXPECT_TRUE(uContains(list, 3));
+    EXPECT_FALSE(uContains(list, 10));
+}
+
+TEST(UStlTest, uContainsMap)
+{
+    std::map<int, std::string> m;
+    m[1] = "a";
+    m[2] = "b";
+    EXPECT_TRUE(uContains(m, 1));
+    EXPECT_FALSE(uContains(m, 10));
+}
+
+TEST(UStlTest, uContainsMultimap)
+{
+    std::multimap<int, std::string> mm;
+    mm.insert({1, "a"});
+    mm.insert({2, "b"});
+    EXPECT_TRUE(uContains(mm, 1));
+    EXPECT_FALSE(uContains(mm, 10));
+}
+
+TEST(UStlTest, uInsert)
+{
+    std::map<int, std::string> m;
+    uInsert(m, std::make_pair(1, std::string("a")));
+    EXPECT_EQ(m[1], "a");
+    
+    uInsert(m, std::make_pair(1, std::string("b")));
+    EXPECT_EQ(m[1], "b"); // Should replace
+    
+    std::map<int, std::string> m2;
+    m2[2] = "c";
+    m2[3] = "d";
+    uInsert(m, m2);
+    EXPECT_EQ(m.size(), 3u);
+}
+
+TEST(UStlTest, uListToVector)
+{
+    std::list<int> list = {1, 2, 3, 4, 5};
+    std::vector<int> v = uListToVector(list);
+    EXPECT_EQ(v.size(), 5u);
+    EXPECT_EQ(v[0], 1);
+    EXPECT_EQ(v[4], 5);
+}
+
+TEST(UStlTest, uVectorToList)
+{
+    std::vector<int> v = {1, 2, 3, 4, 5};
+    std::list<int> list = uVectorToList(v);
+    EXPECT_EQ(list.size(), 5u);
+    EXPECT_EQ(list.front(), 1);
+    EXPECT_EQ(list.back(), 5);
+}
+
+TEST(UStlTest, uMultimapToMap)
+{
+    std::multimap<int, std::string> mm;
+    mm.insert({1, "a"});
+    mm.insert({1, "b"});
+    mm.insert({2, "c"});
+    
+    std::map<int, std::string> m = uMultimapToMap(mm);
+    EXPECT_EQ(m.size(), 2u); // All entries
+}
+
+TEST(UStlTest, uMultimapToMapUnique)
+{
+    std::multimap<int, std::string> mm;
+    mm.insert({1, "a"});
+    mm.insert({1, "b"});
+    mm.insert({2, "c"});
+    
+    std::map<int, std::string> m = uMultimapToMapUnique(mm);
+    EXPECT_EQ(m.size(), 1u); // Only unique keys with single value
+    EXPECT_EQ(m[2], "c");
+}
+
+TEST(UStlTest, uAppend)
+{
+    std::list<int> list1 = {1, 2, 3};
+    std::list<int> list2 = {4, 5, 6};
+    uAppend(list1, list2);
+    EXPECT_EQ(list1.size(), 6u);
+    EXPECT_EQ(list1.back(), 6);
+}
+
+TEST(UStlTest, uIndexOf)
+{
+    std::vector<int> v = {1, 2, 3, 4, 5};
+    EXPECT_EQ(uIndexOf(v, 3), 2);
+    EXPECT_EQ(uIndexOf(v, 10), -1);
+}
+
+TEST(UStlTest, uSplit)
+{
+    std::list<std::string> result = uSplit("Hello the world!", ' ');
+    EXPECT_EQ(result.size(), 3u);
+    EXPECT_EQ(result.front(), "Hello");
+    EXPECT_EQ(result.back(), "world!");
+    
+    std::list<std::string> result2 = uSplit("a,b,c", ',');
+    EXPECT_EQ(result2.size(), 3u);
+}
+
+TEST(UStlTest, uJoin)
+{
+    std::list<std::string> strings;
+    strings.push_back("Hello");
+    strings.push_back("world!");
+    std::string joined = uJoin(strings, " ");
+    EXPECT_EQ(joined, "Hello world!");
+    
+    std::string joined2 = uJoin(strings, "");
+    EXPECT_EQ(joined2, "Helloworld!");
+}
+
+TEST(UStlTest, uIsDigit)
+{
+    EXPECT_TRUE(uIsDigit('0'));
+    EXPECT_TRUE(uIsDigit('5'));
+    EXPECT_TRUE(uIsDigit('9'));
+    EXPECT_FALSE(uIsDigit('a'));
+    EXPECT_FALSE(uIsDigit('A'));
+}
+
+TEST(UStlTest, uIsInteger)
+{
+    EXPECT_TRUE(uIsInteger("123"));
+    EXPECT_TRUE(uIsInteger("-123"));
+    EXPECT_FALSE(uIsInteger("12.3"));
+    EXPECT_FALSE(uIsInteger("abc"));
+    
+    EXPECT_TRUE(uIsInteger("123", true));
+    EXPECT_TRUE(uIsInteger("123", false));
+    EXPECT_FALSE(uIsInteger("-123", false));
+}
+
+TEST(UStlTest, uIsNumber)
+{
+    EXPECT_TRUE(uIsNumber("123"));
+    EXPECT_TRUE(uIsNumber("12.3"));
+    EXPECT_TRUE(uIsNumber("-12.3"));
+    EXPECT_FALSE(uIsNumber("abc"));
+    EXPECT_FALSE(uIsNumber("12.3.4"));
+}
+
+TEST(UStlTest, uSplitNumChar)
+{
+    std::list<std::string> result = uSplitNumChar("Hello 03 my 65 world!");
+    EXPECT_GT(result.size(), 0u);
+    // Should contain numbers and characters separately
+}
+
+TEST(UStlTest, uStrNumCmp)
+{
+    EXPECT_LT(uStrNumCmp("Image9.jpg", "Image10.jpg"), 0);
+    EXPECT_GT(uStrNumCmp("Image10.jpg", "Image9.jpg"), 0);
+    EXPECT_EQ(uStrNumCmp("Image10.jpg", "Image10.jpg"), 0);
+}
+
+TEST(UStlTest, uStrContains)
+{
+    EXPECT_TRUE(uStrContains("Hello World", "World"));
+    EXPECT_TRUE(uStrContains("Hello World", "Hello"));
+    EXPECT_FALSE(uStrContains("Hello World", "xyz"));
+}
+
+TEST(UStlTest, uCompareVersion)
+{
+    EXPECT_GT(uCompareVersion("2.0.0", 1, 0, 0), 0);
+    EXPECT_EQ(uCompareVersion("1.0.0", 1, 0, 0), 0);
+    EXPECT_LT(uCompareVersion("0.9.0", 1, 0, 0), 0);
+}
+
+TEST(UStlTest, uPad)
+{
+    std::string result = uPad("Hello", 10);
+    EXPECT_GE(result.size(), 10u);
+    EXPECT_EQ(result.substr(0, 5), "Hello");
+    
+    std::string result2 = uPad("Hello", 3);
+    EXPECT_EQ(result2, "Hello"); // Should not truncate
+}
+

utilite/test/test_uthread.cpp

@@ -0,0 +1,352 @@
+#include "gtest/gtest.h"
+#include "rtabmap/utilite/UThread.h"
+#include "rtabmap/utilite/UTimer.h"
+#include <thread>
+#include <chrono>
+#include <atomic>
+
+// Simple test thread
+class SimpleThread : public UThread
+{
+public:
+    SimpleThread() : UThread(), counter_(0), started_(false) {}
+    virtual ~SimpleThread() {join(true);}
+    int getCounter() const { return counter_; }
+    bool hasStarted() const { return started_; }
+    void reset() { counter_ = 0; started_ = false; }
+    
+protected:
+    virtual void mainLoop()
+    {
+        started_ = true;
+        counter_++;
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        kill(); // Stop after one iteration
+    }
+    
+private:
+    int counter_;
+    bool started_;
+};
+
+// Thread that runs multiple iterations
+class CountingThread : public UThread
+{
+public:
+    CountingThread() : UThread(), counter_(0), maxIterations_(10) {}
+    virtual ~CountingThread() {join(true);}
+    void setMaxIterations(int max) { maxIterations_ = max; }
+    int getCounter() const { return counter_; }
+    
+protected:
+    virtual void mainLoop()
+    {
+        counter_++;
+        if(counter_ >= maxIterations_)
+        {
+            kill();
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(5));
+    }
+    
+private:
+    int counter_;
+    int maxIterations_;
+};
+
+// Thread with initialization
+class InitThread : public UThread
+{
+public:
+    InitThread() : UThread(), initialized_(false), counter_(0) {}
+    virtual ~InitThread() {join(true);}
+    bool isInitialized() const { return initialized_; }
+    int getCounter() const { return counter_; }
+    
+protected:
+    virtual void mainLoopBegin()
+    {
+        initialized_ = true;
+    }
+    
+    virtual void mainLoop()
+    {
+        counter_++;
+        if(counter_ >= 5)
+        {
+            kill();
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(5));
+    }
+    
+private:
+    bool initialized_;
+    int counter_;
+};
+
+// Thread with cleanup
+class CleanupThread : public UThread
+{
+public:
+    CleanupThread() : UThread(), cleanedUp_(false), counter_(0) {}
+    virtual ~CleanupThread() {join(true);}
+    bool isCleanedUp() const { return cleanedUp_; }
+    int getCounter() const { return counter_; }
+    
+protected:
+    virtual void mainLoop()
+    {
+        counter_++;
+        if(counter_ >= 3)
+        {
+            kill();
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(5));
+    }
+    
+    virtual void mainLoopEnd()
+    {
+        cleanedUp_ = true;
+    }
+    
+private:
+    bool cleanedUp_;
+    int counter_;
+};
+
+TEST(UThreadTest, Constructor)
+{
+    SimpleThread thread;
+    EXPECT_TRUE(thread.isIdle());
+    EXPECT_FALSE(thread.isRunning());
+    EXPECT_FALSE(thread.isCreating());
+    EXPECT_FALSE(thread.isKilled());
+}
+
+TEST(UThreadTest, Start)
+{
+    SimpleThread thread;
+    thread.start();
+
+    EXPECT_TRUE(thread.isRunning() || thread.isCreating());
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(5));
+    EXPECT_TRUE(thread.isRunning());
+    
+    // Give thread time to stop
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    
+    // Thread should be running or killed (if it finished quickly)
+    EXPECT_TRUE(thread.isKilled() || thread.isIdle());
+    
+    thread.join();
+}
+
+TEST(UThreadTest, Join)
+{
+    SimpleThread thread;
+    UTimer timer;
+    thread.start();
+    thread.join();
+    
+    EXPECT_GT(timer.elapsed(), 0.01);
+    EXPECT_TRUE(thread.isIdle());
+    EXPECT_GT(thread.getCounter(), 0);
+}
+
+TEST(UThreadTest, JoinWait)
+{
+    CountingThread thread;
+    thread.setMaxIterations(10);
+    thread.start();
+    
+    // Join wait
+    thread.join(false);
+    
+    EXPECT_TRUE(thread.isIdle());
+    EXPECT_EQ(thread.getCounter(), 10); // Should have processed all iterations
+}
+
+TEST(UThreadTest, JoinKillFirst)
+{
+    CountingThread thread;
+    thread.setMaxIterations(1000);
+    thread.start();
+    
+    // Wait a bit
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    
+    // Join with kill
+    thread.join(true);
+    
+    EXPECT_TRUE(thread.isIdle());
+    EXPECT_LT(thread.getCounter(), 1000); // Should have stopped early
+}
+
+TEST(UThreadTest, Kill)
+{
+    CountingThread thread;
+    thread.setMaxIterations(1000);
+    thread.start();
+    
+    // Wait a bit
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    
+    // Kill the thread
+    thread.kill();
+
+    EXPECT_TRUE(thread.isKilled());
+    
+    // Wait for it to finish
+    thread.join();
+    
+    EXPECT_TRUE(thread.isIdle());
+    EXPECT_LT(thread.getCounter(), 1000); // Should have stopped early
+}
+
+TEST(UThreadTest, MultipleStart)
+{
+    SimpleThread thread;
+    thread.start();
+    
+    // Try to start again (should be ignored)
+    thread.start();
+    
+    thread.join();
+    
+    EXPECT_TRUE(thread.isIdle());
+}
+
+TEST(UThreadTest, MainLoopBegin)
+{
+    InitThread thread;
+    thread.start();
+    thread.join();
+    
+    EXPECT_TRUE(thread.isInitialized());
+    EXPECT_GT(thread.getCounter(), 0);
+}
+
+TEST(UThreadTest, MainLoopEnd)
+{
+    CleanupThread thread;
+    thread.start();
+    thread.join();
+    
+    EXPECT_TRUE(thread.isCleanedUp());
+    EXPECT_GT(thread.getCounter(), 0);
+}
+
+TEST(UThreadTest, ThreadId)
+{
+    SimpleThread thread;
+    unsigned long mainThreadId = UThread::currentThreadId();
+    
+    thread.start();
+    unsigned long threadId = thread.getThreadId();
+    
+    EXPECT_NE(threadId, 0u);
+    EXPECT_NE(threadId, mainThreadId);
+    
+    thread.join();
+}
+
+TEST(UThreadTest, CurrentThreadId)
+{
+    unsigned long id1 = UThread::currentThreadId();
+    unsigned long id2 = UThread::currentThreadId();
+    
+    EXPECT_EQ(id1, id2);
+    EXPECT_NE(id1, 0u);
+}
+
+TEST(UThreadTest, SetPriority)
+{
+    SimpleThread thread;
+    thread.setPriority(UThread::kPLow);
+    thread.setPriority(UThread::kPNormal);
+    thread.setPriority(UThread::kPAboveNormal);
+    
+    thread.start();
+    thread.join();
+    
+    SUCCEED();
+}
+
+TEST(UThreadTest, SetAffinity)
+{
+    SimpleThread thread;
+    thread.setAffinity(0); // No affinity
+    thread.setAffinity(1); // CPU 1
+    
+    thread.start();
+    thread.join();
+    
+    SUCCEED();
+}
+
+TEST(UThreadTest, ThreadHandle)
+{
+    SimpleThread thread;
+    thread.start();
+    
+    UThread::Handle handle = thread.getThreadHandle();
+    EXPECT_NE(handle, 0);
+    
+    thread.join();
+}
+
+TEST(UThreadTest, MultipleThreads)
+{
+    std::vector<SimpleThread*> threads;
+    const int numThreads = 5;
+    
+    for(int i = 0; i < numThreads; ++i)
+    {
+        threads.push_back(new SimpleThread());
+        threads[i]->start();
+    }
+    
+    for(int i = 0; i < numThreads; ++i)
+    {
+        threads[i]->join();
+        EXPECT_GT(threads[i]->getCounter(), 0);
+        delete threads[i];
+    }
+}
+
+TEST(UThreadTest, DestructorJoin)
+{
+    // Test that destructor properly cleans up
+    {
+        SimpleThread thread;
+        thread.start();
+        // Destructor should call join(true) if implemented
+    }
+    
+    SUCCEED();
+}
+
+TEST(UThreadTest, KillBeforeStart)
+{
+    SimpleThread thread;
+    thread.kill(); // Kill before start
+    
+    thread.start();
+    thread.join();
+    
+    // Should still work
+    SUCCEED();
+}
+
+TEST(UThreadTest, JoinBeforeStart)
+{
+    SimpleThread thread;
+    thread.join(); // Join before start
+    
+    thread.start();
+    thread.join();
+    
+    SUCCEED();
+}
+

utilite/test/test_utimer.cpp

@@ -0,0 +1,115 @@
+#include "gtest/gtest.h"
+#include "rtabmap/utilite/UTimer.h"
+#include <thread>
+#include <chrono>
+
+TEST(UTimerTest, Constructor)
+{
+    UTimer timer;
+    // Timer should be constructible
+    SUCCEED();
+}
+
+TEST(UTimerTest, StartStop)
+{
+    UTimer timer;
+    timer.start();
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    timer.stop();
+    double elapsed = timer.getElapsedTime();
+    EXPECT_GE(elapsed, 0.01); // Should be at least 10 ms
+    EXPECT_LT(elapsed, 0.015); // Should be less than 15 ms
+}
+
+TEST(UTimerTest, Elapsed)
+{
+    UTimer timer;
+    timer.start();
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    double elapsed = timer.elapsed();
+    EXPECT_GE(elapsed, 0.01);
+    EXPECT_LT(elapsed, 0.015);
+
+    // Timer should still be running after elapsed()
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    double ticks2 = timer.ticks();
+    EXPECT_GE(ticks2, 0.02);
+    EXPECT_LT(ticks2, 0.025);
+}
+
+TEST(UTimerTest, GetElapsedTime)
+{
+    UTimer timer;
+    timer.start();
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    double elapsed = timer.getElapsedTime();
+    EXPECT_GE(elapsed, 0.01);
+    EXPECT_LT(elapsed, 0.015);
+
+    // Timer should still be running after getElapsedTime()
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    double ticks2 = timer.ticks();
+    EXPECT_GE(ticks2, 0.02);
+    EXPECT_LT(ticks2, 0.025);
+}
+
+TEST(UTimerTest, Ticks)
+{
+    UTimer timer;
+    timer.start();
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    double ticks1 = timer.ticks();
+    EXPECT_GE(ticks1, 0.01);
+    EXPECT_LT(ticks1, 0.015);
+    
+    // Timer should have retarted and still be running after ticks()
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    double ticks2 = timer.ticks();
+    EXPECT_GE(ticks2, 0.01);
+    EXPECT_LT(ticks2, 0.015);
+}
+
+TEST(UTimerTest, Restart)
+{
+    UTimer timer;
+    timer.start();
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    double elapsed1 = timer.restart();
+    EXPECT_GE(elapsed1, 0.01);
+    EXPECT_LT(elapsed1, 0.02);
+    
+    // After restart, timer should be running again
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    double elapsed2 = timer.elapsed();
+    EXPECT_GE(elapsed2, 0.01);
+    EXPECT_LT(elapsed2, 0.02);
+}
+
+TEST(UTimerTest, Now)
+{
+    double time1 = UTimer::now();
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    double time2 = UTimer::now();
+    EXPECT_GE(time2-time1, 0.01);
+    EXPECT_LT(time2-time1, 0.015);
+}
+
+TEST(UTimerTest, MultipleStartStop)
+{
+    UTimer timer;
+    timer.start();
+    std::this_thread::sleep_for(std::chrono::milliseconds(5));
+    timer.stop();
+    double elapsed1 = timer.getElapsedTime();
+    
+    timer.start();
+    std::this_thread::sleep_for(std::chrono::milliseconds(5));
+    timer.stop();
+    double elapsed2 = timer.getElapsedTime();
+    
+    EXPECT_GE(elapsed1, 0.005);
+    EXPECT_LT(elapsed1, 0.01);
+    EXPECT_GE(elapsed2, 0.005);
+    EXPECT_LT(elapsed2, 0.01);
+}
+

utilite/test/test_uvariant.cpp

@@ -0,0 +1,213 @@
+#include "gtest/gtest.h"
+#include "rtabmap/utilite/UVariant.h"
+
+TEST(UVariantTest, DefaultConstructor)
+{
+    UVariant v;
+    EXPECT_TRUE(v.isUndef());
+    EXPECT_EQ(v.type(), UVariant::kUndef);
+}
+
+TEST(UVariantTest, BoolConstructor)
+{
+    UVariant v(true);
+    EXPECT_TRUE(v.isBool());
+    EXPECT_EQ(v.type(), UVariant::kBool);
+    EXPECT_TRUE(v.toBool());
+    
+    UVariant v2(false);
+    EXPECT_FALSE(v2.toBool());
+}
+
+TEST(UVariantTest, CharConstructor)
+{
+    signed char value = 42;
+    UVariant v(value);
+    EXPECT_TRUE(v.isChar());
+    EXPECT_EQ(v.type(), UVariant::kChar);
+    bool ok = false;
+    EXPECT_EQ(v.toChar(&ok), value);
+    EXPECT_TRUE(ok);
+}
+
+TEST(UVariantTest, UCharConstructor)
+{
+    unsigned char value = 200;
+    UVariant v(value);
+    EXPECT_TRUE(v.isUChar());
+    EXPECT_EQ(v.type(), UVariant::kUChar);
+    bool ok = false;
+    EXPECT_EQ(v.toUChar(&ok), value);
+    EXPECT_TRUE(ok);
+}
+
+TEST(UVariantTest, ShortConstructor)
+{
+    short value = -100;
+    UVariant v(value);
+    EXPECT_TRUE(v.isShort());
+    EXPECT_EQ(v.type(), UVariant::kShort);
+    bool ok = false;
+    EXPECT_EQ(v.toShort(&ok), value);
+    EXPECT_TRUE(ok);
+}
+
+TEST(UVariantTest, UShortConstructor)
+{
+    unsigned short value = 500;
+    UVariant v(value);
+    EXPECT_TRUE(v.isUShort());
+    EXPECT_EQ(v.type(), UVariant::kUShort);
+    bool ok = false;
+    EXPECT_EQ(v.toUShort(&ok), value);
+    EXPECT_TRUE(ok);
+}
+
+TEST(UVariantTest, IntConstructor)
+{
+    int value = -1000;
+    UVariant v(value);
+    EXPECT_TRUE(v.isInt());
+    EXPECT_EQ(v.type(), UVariant::kInt);
+    bool ok = false;
+    EXPECT_EQ(v.toInt(&ok), value);
+    EXPECT_TRUE(ok);
+}
+
+TEST(UVariantTest, UIntConstructor)
+{
+    unsigned int value = 100000;
+    UVariant v(value);
+    EXPECT_TRUE(v.isUInt());
+    EXPECT_EQ(v.type(), UVariant::kUInt);
+    bool ok = false;
+    EXPECT_EQ(v.toUInt(&ok), value);
+    EXPECT_TRUE(ok);
+}
+
+TEST(UVariantTest, FloatConstructor)
+{
+    float value = 3.14f;
+    UVariant v(value);
+    EXPECT_TRUE(v.isFloat());
+    EXPECT_EQ(v.type(), UVariant::kFloat);
+    bool ok = false;
+    EXPECT_NEAR(v.toFloat(&ok), value, 0.001f);
+    EXPECT_TRUE(ok);
+}
+
+TEST(UVariantTest, DoubleConstructor)
+{
+    double value = 3.14159;
+    UVariant v(value);
+    EXPECT_TRUE(v.isDouble());
+    EXPECT_EQ(v.type(), UVariant::kDouble);
+    bool ok = false;
+    EXPECT_NEAR(v.toDouble(&ok), value, 0.00001);
+    EXPECT_TRUE(ok);
+}
+
+TEST(UVariantTest, StringConstructor)
+{
+    std::string value = "Hello World";
+    UVariant v(value);
+    EXPECT_TRUE(v.isStr());
+    EXPECT_EQ(v.type(), UVariant::kStr);
+    bool ok = false;
+    EXPECT_EQ(v.toStr(&ok), value);
+    EXPECT_TRUE(ok);
+}
+
+TEST(UVariantTest, CStringConstructor)
+{
+    const char* value = "Hello World";
+    UVariant v(value);
+    EXPECT_TRUE(v.isStr());
+    EXPECT_EQ(v.type(), UVariant::kStr);
+    bool ok = false;
+    EXPECT_EQ(v.toStr(&ok), std::string(value));
+    EXPECT_TRUE(ok);
+}
+
+TEST(UVariantTest, CharArrayConstructor)
+{
+    std::vector<signed char> value = {1, 2, 3, -4};
+    UVariant v(value);
+    EXPECT_TRUE(v.isCharArray());
+    EXPECT_EQ(v.type(), UVariant::kCharArray);
+    bool ok = false;
+    std::vector<signed char> result = v.toCharArray(&ok);
+    EXPECT_TRUE(ok);
+    EXPECT_EQ(result.size(), value.size());
+    for(size_t i = 0; i < value.size(); ++i)
+    {
+        EXPECT_EQ(result[i], value[i]);
+    }
+}
+
+TEST(UVariantTest, UCharArrayConstructor)
+{
+    std::vector<unsigned char> value = {1, 2, 3, 4};
+    UVariant v(value);
+    EXPECT_TRUE(v.isUCharArray());
+    EXPECT_EQ(v.type(), UVariant::kUCharArray);
+    bool ok = false;
+    std::vector<unsigned char> result = v.toUCharArray(&ok);
+    EXPECT_TRUE(ok);
+    EXPECT_EQ(result.size(), value.size());
+    for(size_t i = 0; i < value.size(); ++i)
+    {
+        EXPECT_EQ(result[i], value[i]);
+    }
+}
+
+TEST(UVariantTest, IntArrayConstructor)
+{
+    std::vector<int> value = {1, -2, 3, -4, 5};
+    UVariant v(value);
+    EXPECT_TRUE(v.isIntArray());
+    EXPECT_EQ(v.type(), UVariant::kIntArray);
+    bool ok = false;
+    std::vector<int> result = v.toIntArray(&ok);
+    EXPECT_TRUE(ok);
+    EXPECT_EQ(result.size(), value.size());
+    for(size_t i = 0; i < value.size(); ++i)
+    {
+        EXPECT_EQ(result[i], value[i]);
+    }
+}
+
+TEST(UVariantTest, FloatArrayConstructor)
+{
+    std::vector<float> value = {1.1f, 2.2f, 3.3f};
+    UVariant v(value);
+    EXPECT_TRUE(v.isFloatArray());
+    EXPECT_EQ(v.type(), UVariant::kFloatArray);
+    bool ok = false;
+    std::vector<float> result = v.toFloatArray(&ok);
+    EXPECT_TRUE(ok);
+    EXPECT_EQ(result.size(), value.size());
+    for(size_t i = 0; i < value.size(); ++i)
+    {
+        EXPECT_NEAR(result[i], value[i], 0.001f);
+    }
+}
+
+TEST(UVariantTest, TypeChecks)
+{
+    UVariant v(42);
+    EXPECT_TRUE(v.isInt());
+    EXPECT_FALSE(v.isBool());
+    EXPECT_FALSE(v.isFloat());
+    EXPECT_FALSE(v.isStr());
+    EXPECT_FALSE(v.isUndef());
+}
+
+TEST(UVariantTest, ConversionFailure)
+{
+    UVariant v(42); // int
+    bool ok = true;
+    v.toFloat(&ok);
+    EXPECT_FALSE(ok); // Should fail to convert int to float
+}
+