CI coverage: adding g2o, gtsam, ceres and libpointmatcher. (#1743)

* CI coverage: adding g2o, gtsam, ceres and libpointmatcher. Also fetch test data for integration tests.

* ld

* Increasing ci test timeout for coverage

* Fixed tests when vertigo is not there

* verbose tests

* updating flaky test

* faster coverage by extending unit tests and disable integration test (only for coverage report)

* improving coverage of unit tests in comparison to integrations tests

* fixing not covered new lines

.github/workflows/cmake-linux.yml

@@ -101,4 +101,4 @@ jobs:
       env:
         PYTHONNOUSERSITE: 1
       run: |
-        ctest -C ${{env.BUILD_TYPE}} --output-on-failure
+        ctest -C ${{env.BUILD_TYPE}} -V

.github/workflows/cmake-macos.yml

@@ -124,7 +124,7 @@ jobs:
       env:
         PYTHONNOUSERSITE: 1
       run: |
-        ctest -C ${{env.BUILD_TYPE}} --output-on-failure
+        ctest -C ${{env.BUILD_TYPE}} -V
 
     # A ctest SEGFAULT is reported as just "SEGFAULT" with no backtrace, which
     # makes a crash inside a long integration test invisible -- the log simply

.github/workflows/cmake-windows.yml

@@ -125,7 +125,7 @@ jobs:
         PYTHONHOME: ${{env.VCPKG_EXPORT_PATH}}/installed/x64-windows-release/tools/python3
         PYTHONNOUSERSITE: 1
       run: |
-        ctest -C ${{env.BUILD_TYPE}} --output-on-failure --timeout 300
+        ctest -C ${{env.BUILD_TYPE}} -V --timeout 300
 
     - name: Info
       # Skipped for CUDA: the binary links ZED (sl_zed64.dll -> nvcuvid/nvEncodeAPI64),

.github/workflows/coverage.yml

@@ -30,6 +30,19 @@ jobs:
     steps:
       - uses: actions/checkout@v4
 
+      - name: Restore test data
+        id: cache-testdata
+        uses: actions/cache@v4
+        with:
+          # Same key as the other workflows so all of them share one entry.
+          path: data/tests/*.db
+          key: testdata-${{ hashFiles('data/tests/manifest.txt') }}
+
+      - name: Fetch test data
+        if: steps.cache-testdata.outputs.cache-hit != 'true'
+        shell: bash
+        run: bash scripts/fetch_test_data.sh
+
       - name: Install Linux dependencies
         run: |
           DEBIAN_FRONTEND=noninteractive
@@ -41,26 +54,45 @@ jobs:
             git \
             cmake \
             software-properties-common \
-            libyaml-cpp-dev
+            libyaml-cpp-dev \
+            libg2o-dev \
+            libceres-dev \
+            curl \
+            gnupg \
+            lsb-release
+
+          # GTSAM and libpointmatcher are not in the Ubuntu archive, and
+          # borglab's gtsam PPAs have no noble packages (gtsam-release-4.2 has
+          # no noble suite; gtsam-develop's noble Packages index is empty). The
+          # ROS 2 repo ships both for noble, which is also what the ROS CI jobs
+          # test against, so take them from there.
+          sudo curl -fsSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \
+            -o /usr/share/keyrings/ros-archive-keyring.gpg
+          echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" \
+            | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
+          sudo apt-get update
+          sudo apt-get install -y \
+            ros-jazzy-gtsam \
+            ros-jazzy-libpointmatcher
 
       - name: Configure CMake
         run: |
           cmake -B ${{ github.workspace }}/build \
             -DCMAKE_BUILD_TYPE=${{ env.BUILD_TYPE }} \
             -DENABLE_COVERAGE=ON \
+            -DCMAKE_PREFIX_PATH=/opt/ros/jazzy \
             -DBUILD_TESTING=ON \
             -DBUILD_APP=OFF \
             -DBUILD_TOOLS=OFF \
             -DBUILD_EXAMPLES=OFF \
             -DWITH_QT=OFF \
             -DWITH_PYTHON=OFF \
-            -DWITH_CERES=OFF \
+            -DWITH_CERES=ON \
-            -DWITH_G2O=OFF \
+            -DWITH_G2O=ON \
-            -DWITH_GTSAM=OFF \
+            -DWITH_GTSAM=ON \
             -DWITH_MRPT=OFF \
-            -DWITH_VERTIGO=OFF \
             -DWITH_CVSBA=OFF \
-            -DWITH_POINTMATCHER=OFF \
+            -DWITH_POINTMATCHER=ON \
             -DWITH_CCCORELIB=OFF \
             -DWITH_OPEN3D=OFF \
             -DWITH_LOAM=OFF \
@@ -77,9 +109,26 @@ jobs:
       - name: Test
         working-directory: ${{ github.workspace }}/build
         run: |
-          export LD_LIBRARY_PATH="${{ github.workspace }}/build/bin${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}"
+          # The ROS packages are not on the default loader path. Both
+          # directories are needed: GTSAM and libpointmatcher land in
+          # /opt/ros/jazzy/lib, but gtsam's own libmetis-gtsam.so goes to the
+          # multiarch subdirectory, and libgtsam.so carries no RUNPATH, so the
+          # loader can only find it through LD_LIBRARY_PATH.
+          ROS_LIB=/opt/ros/jazzy/lib
+          export LD_LIBRARY_PATH="${{ github.workspace }}/build/bin:${ROS_LIB}:${ROS_LIB}/$(gcc -dumpmachine)${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}"
           find ${{ github.workspace }}/build -name '*.gcda' -delete
-          ctest --output-on-failure
+          # Unit tests only ("long" is the end-to-end replay suite).
+          #
+          # Two reasons. It costs ~90 min in this instrumented -O0 build, 94% of
+          # the job's test time, for ~3 points of coverage. And running it on
+          # some events but not others makes the numbers incomparable: Codecov
+          # measures a pull request against master, so a unit-only PR report
+          # against a full master report reads as a coverage regression on every
+          # PR. Same scope everywhere keeps the comparison meaningful.
+          #
+          # The replays still run (and gate) in the cmake-linux / macos /
+          # windows jobs; they are just not measured here.
+          ctest -V -LE long
 
       - name: Generate LCOV report
         run: |

corelib/include/rtabmap/core/DBDriver.h

@@ -93,7 +93,18 @@ public:
 	/** @return Target schema version for new databases (from parameters). */
 	const std::string & getTargetVersion() const {return _targetVersion;}
 
-	/** @brief Queue a signature for deferred save; ownership is transferred. */
+	/**
+	 * @brief Queue a signature for deferred save; ownership is transferred.
+	 *
+	 * @note Only the *compressed* sensor buffers are written
+	 * (SensorData::imageCompressed(), depthOrRightCompressed(),
+	 * laserScanCompressed(), ...). Raw matrices are ignored, so a signature
+	 * carrying only raw data is stored with empty payloads. Memory compresses
+	 * before saving; a caller driving the driver directly should compress first
+	 * with @ref compressImage2() / @ref compressData2(), or pass the compressed
+	 * buffers to SensorData::setRGBDImage() / setLaserScan(), which treat a
+	 * 1-row CV_8UC1 matrix as already compressed.
+	 */
 	void asyncSave(Signature * s);
 	/** @brief Queue a visual word for deferred save; ownership is transferred. */
 	void asyncSave(VisualWord * vw);

corelib/src/DBDriverSqlite3.cpp

@@ -4669,6 +4669,14 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
 
 			for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
 			{
+				if(((*i)->sensorData().imageCompressed().empty() && !(*i)->sensorData().imageRaw().empty()) ||
+				   ((*i)->sensorData().depthOrRightCompressed().empty() && !(*i)->sensorData().depthOrRightRaw().empty()) ||
+				   ((*i)->sensorData().laserScanCompressed().isEmpty() && !(*i)->sensorData().laserScanRaw().isEmpty()))
+				{
+					UWARN("Node %d carries raw sensor data but no compressed data. Only compressed "
+						  "buffers are saved, so that payload will be empty in the database. Compress "
+						  "it first (see compressImage2() / compressData2()).", (*i)->id());
+				}
 				if(!(*i)->sensorData().imageCompressed().empty() ||
 				   !(*i)->sensorData().depthOrRightCompressed().empty() ||
 				   !(*i)->sensorData().depthConfidenceCompressed().empty() ||

corelib/src/Memory.cpp

@@ -3850,7 +3850,12 @@ Transform Memory::computeIcpTransformMulti(
 			guessNorm > fromScan.rangeMax() + toScan.rangeMax())
 		{
 			// stop right known,it is impossible that scans overlay.
-			UINFO("Too far scans between %d and %d to compute transformation: guessNorm=%f, scan range from=%f to=%f", fromId, toId, guessNorm, fromScan.rangeMax(), toScan.rangeMax());
+			const std::string rejected = uFormat("Too far scans between %d and %d to compute transformation: guessNorm=%f, scan range from=%f to=%f", fromId, toId, guessNorm, fromScan.rangeMax(), toScan.rangeMax());
+			UINFO("%s", rejected.c_str());
+			if(info)
+			{
+				info->rejectedMsg = rejected;
+			}
 			return t;
 		}
 
@@ -3989,6 +3994,12 @@ Transform Memory::computeIcpTransformMulti(
 			t = t.inverse();
 		}
 	}
+	else if(info)
+	{
+		info->rejectedMsg = uFormat("Node %d (scan %s) or %d (scan %s) has no laser scan, cannot compute ICP transform.",
+				fromId, fromScan.isEmpty()?"empty":"ok",
+				toId, toScan.isEmpty()?"empty":"ok");
+	}
 
 	return t;
 }

corelib/test/CMakeLists.txt

@@ -62,6 +62,7 @@ set(corelib_test_sources
 	test_sensorevent.cpp                #SensorEvent.h
 	test_sensordata.cpp                 #SensorData.h
 	test_sensorcapture.cpp              #SensorCapture.h
+	test_cameraimages.cpp               #camera/CameraImages.h
 	test_sensorcaptureinfo.cpp          #SensorCaptureInfo.h
 	test_sensorcapturethread.cpp        #SensorCaptureThread.h
 )
@@ -76,6 +77,14 @@ IF(libpointmatcher_FOUND)
 	target_link_libraries(test_corelib ${libpointmatcher_LIBRARIES})
 ENDIF(libpointmatcher_FOUND)
 
+# The timeouts below are sized for an optimized build; at -O0 (Debug, or
+# coverage, which forces -O0) these tests run 5-40x slower.
+IF(CMAKE_BUILD_TYPE MATCHES "^(Release|RelWithDebInfo|MinSizeRel)$")
+	SET(_test_timeout_scale 1)
+ELSE()
+	SET(_test_timeout_scale 6)
+ENDIF()
+
 # Split the run across processes so `ctest -j` still overlaps work, paying the
 # library load N times instead of once per source file. gtest partitions by
 # test index via these two env vars; the shards are disjoint and together
@@ -85,10 +94,11 @@ ENDIF(libpointmatcher_FOUND)
 set(CORELIB_TEST_SHARDS 4 CACHE STRING
 		"Number of processes the corelib unit tests are split across")
 math(EXPR _corelib_last_shard "${CORELIB_TEST_SHARDS} - 1")
+math(EXPR _corelib_shard_timeout "900 * ${_test_timeout_scale}")
 foreach(shard RANGE 0 ${_corelib_last_shard})
 	add_test(NAME test_corelib_${shard} COMMAND test_corelib)
 	set_tests_properties(test_corelib_${shard} PROPERTIES
-			TIMEOUT 900
+			TIMEOUT ${_corelib_shard_timeout}
 			ENVIRONMENT "GTEST_TOTAL_SHARDS=${CORELIB_TEST_SHARDS};GTEST_SHARD_INDEX=${shard}")
 endforeach()
 
@@ -107,8 +117,9 @@ target_link_libraries(test_rtabmap_integration gtest_main rtabmap_core)
 # while nightlies run:
 #  `ctest -L long`.
 add_test(NAME test_rtabmap_integration COMMAND test_rtabmap_integration)
+math(EXPR _integration_timeout "1800 * ${_test_timeout_scale}")
 set_tests_properties(test_rtabmap_integration PROPERTIES
-        TIMEOUT 1800
+        TIMEOUT ${_integration_timeout}
         LABELS  "long")
 
 #PythonInterface / PyDetector / PyDescriptor / PyMatcher (optional Python

corelib/test/test_cameraimages.cpp

@@ -0,0 +1,93 @@
+
+
+#include <gtest/gtest.h>
+#include <rtabmap/core/camera/CameraImages.h>
+#include <rtabmap/core/SensorData.h>
+#include <rtabmap/core/SensorCaptureInfo.h>
+#include <rtabmap/core/Transform.h>
+#include <opencv2/core.hpp>
+#include <string>
+
+using namespace rtabmap;
+
+// ---------------------------------------------------------------------------
+// CameraImages over a real image directory. The tests above use a mock capture
+// to cover the SensorCapture base class (frame-rate throttling, local
+// transform, info filling); this drives the concrete directory-scanning
+// implementation over the 84 committed images in data/samples.
+// ---------------------------------------------------------------------------
+
+
+TEST(CameraImagesTest, ScansSampleDirectoryAndCapturesFrames)
+{
+	const std::string path = std::string(RTABMAP_TEST_DATA_ROOT) + "/samples";
+	CameraImages camera(path);
+	ASSERT_TRUE(camera.init()) << "could not scan " << path;
+	// data/samples ships no calibration, so the model stays unnamed and
+	// getSerial() (which returns the model name) is empty -- the two must agree.
+	EXPECT_FALSE(camera.isCalibrated());
+	EXPECT_TRUE(camera.getSerial().empty());
+
+	// Take a handful rather than all 84: enough to exercise the scan/decode
+	// loop and the frame ordering without paying for the whole directory.
+	Transform previousStamp;
+	int frames = 0;
+	double lastStamp = -1.0;
+	for(int i = 0; i < 5; ++i)
+	{
+		SensorCaptureInfo info;
+		SensorData data = camera.takeData(&info);
+		if(data.imageRaw().empty() && data.imageCompressed().empty())
+		{
+			break;
+		}
+		++frames;
+		EXPECT_GT(data.imageRaw().cols, 0);
+		EXPECT_GT(data.imageRaw().rows, 0);
+		EXPECT_GT(data.stamp(), lastStamp) << "stamps must increase across frames";
+		lastStamp = data.stamp();
+	}
+	EXPECT_EQ(5, frames) << "expected 5 frames from a directory of 84 images";
+}
+
+// setStartIndex()/setMaxFrames() are how callers replay a slice of a
+// directory; without them the reader always starts at the first file.
+TEST(CameraImagesTest, HonoursStartIndexAndMaxFrames)
+{
+	const std::string path = std::string(RTABMAP_TEST_DATA_ROOT) + "/samples";
+
+	CameraImages fromStart(path);
+	ASSERT_TRUE(fromStart.init());
+	SensorData first = fromStart.takeData();
+	ASSERT_FALSE(first.imageRaw().empty());
+
+	CameraImages skipped(path);
+	skipped.setStartIndex(10);
+	skipped.setMaxFrames(2);
+	ASSERT_TRUE(skipped.init());
+
+	SensorData tenth = skipped.takeData();
+	ASSERT_FALSE(tenth.imageRaw().empty()) << "start index 10 returned nothing";
+	// Different file, so the decoded pixels should differ from frame 0.
+	EXPECT_NE(0.0, cv::norm(first.imageRaw(), tenth.imageRaw(), cv::NORM_L1))
+			<< "start index had no effect: same image as the first frame";
+
+	EXPECT_FALSE(skipped.takeData().imageRaw().empty()) << "second of maxFrames=2 missing";
+	EXPECT_TRUE(skipped.takeData().imageRaw().empty()) << "maxFrames=2 should stop after two frames";
+}
+
+// The default constructor takes no path; a caller uses setPath()/setStartIndex()
+// before init(). Covered separately because the path constructor above runs a
+// different initialiser list.
+TEST(CameraImagesTest, DefaultConstructedIsUnconfigured)
+{
+	CameraImages camera;
+	EXPECT_FALSE(camera.isCalibrated());
+	EXPECT_TRUE(camera.getSerial().empty());
+	// No directory yet, so scanning must fail rather than assert.
+	EXPECT_FALSE(camera.init());
+
+	camera.setPath(std::string(RTABMAP_TEST_DATA_ROOT) + "/samples");
+	ASSERT_TRUE(camera.init()) << "init() failed after setPath()";
+	EXPECT_FALSE(camera.takeData().imageRaw().empty());
+}

corelib/test/test_dbdriversqlite3.cpp

@@ -3,6 +3,15 @@
 #include <rtabmap/core/DBDriverSqlite3.h>
 #include <rtabmap/core/Signature.h>
 #include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/DBReader.h>
+#include <rtabmap/core/SensorCaptureInfo.h>
+#include <rtabmap/core/CameraModel.h>
+#include <rtabmap/core/LaserScan.h>
+#include <rtabmap/core/Compression.h>
+#include <rtabmap/core/Link.h>
+#include <rtabmap/core/Link.h>
+#include <opencv2/core.hpp>
+#include <memory>
 #include <rtabmap/utilite/UFile.h>
 #include <rtabmap/utilite/UConversion.h>
 #include "TestUtils.h"
@@ -174,3 +183,261 @@ TEST_F(DBDriverSqlite3Fixture, ExecuteNoResultPragma)
 	EXPECT_NO_THROW(driver_->executeNoResult("PRAGMA cache_size=8000;"));
 	EXPECT_TRUE(driver_->isConnected());
 }
+
+// ---------------------------------------------------------------------------
+// Full write/read round trip: build a small database with rich sensor data,
+// then replay it with DBReader.
+//
+// The tests above save bare Signatures, which exercises the schema but not the
+// payload paths (compressed image/depth/scan blobs, calibration, links) nor
+// DBReader at all -- until now those only ran during the end-to-end replay
+// tests, which need the fetched sample databases.
+// ---------------------------------------------------------------------------
+
+namespace {
+
+// A frame carrying everything the payload paths serialise: RGB, depth,
+// calibration and a laser scan.
+SensorData makeRichData(int id, double stamp)
+{
+	cv::Mat rgb(60, 80, CV_8UC3);
+	cv::randu(rgb, cv::Scalar::all(0), cv::Scalar::all(255));
+	cv::Mat depth(60, 80, CV_16UC1);
+	cv::randu(depth, cv::Scalar::all(500), cv::Scalar::all(4000));
+	const CameraModel model(100.0, 100.0, 40.0, 30.0, Transform::getIdentity(), 0.0, cv::Size(80, 60));
+
+	cv::Mat scanData(1, 50, CV_32FC3);
+	for(int i = 0; i < 50; ++i)
+	{
+		scanData.at<cv::Vec3f>(0, i) = cv::Vec3f(0.01f * i, 0.02f * i, 0.0f);
+	}
+
+	// The driver persists the *compressed* buffers, so compress up front the
+	// way Memory does before saving; raw-only data would be written as empty
+	// blobs. setRGBDImage()/setLaserScan() detect a 1-row CV_8UC1 as compressed.
+	SensorData data;
+	data.setId(id);
+	data.setStamp(stamp);
+	data.setRGBDImage(compressImage2(rgb, ".png"), compressImage2(depth, ".png"), model);
+	data.setLaserScan(LaserScan(compressData2(scanData), /*maxPoints=*/0, /*maxRange=*/0.0f,
+			LaserScan::kXYZ));
+	return data;
+}
+
+}  // namespace
+
+TEST_F(DBDriverSqlite3Fixture, SavesAndLoadsRichSensorData)
+{
+	const Transform pose(1.0f, 2.0f, 0.0f, 0.0f, 0.0f, 0.3f);
+	Signature * s = new Signature(10, 0, 1, 1.5, "node10", pose, Transform(), makeRichData(10, 1.5));
+	saveSignature(s);
+
+	std::list<Signature *> loaded;
+	driver_->loadSignatures(std::list<int>(1, 10), loaded);
+	ASSERT_EQ(1u, loaded.size());
+	Signature * back = loaded.front();
+	EXPECT_EQ(10, back->id());
+	EXPECT_EQ(0, back->mapId());
+	EXPECT_DOUBLE_EQ(1.5, back->getStamp());
+	EXPECT_EQ("node10", back->getLabel());
+	EXPECT_LT(back->getPose().getDistance(pose), 1e-4f);
+
+	// Payloads come back compressed; ask the driver to fill them in.
+	std::list<Signature *> toFill(1, back);
+	driver_->loadNodeData(toFill);
+	back->sensorData().uncompressData();
+	EXPECT_FALSE(back->sensorData().imageRaw().empty()) << "image blob did not round-trip";
+	EXPECT_FALSE(back->sensorData().depthRaw().empty()) << "depth blob did not round-trip";
+	EXPECT_FALSE(back->sensorData().laserScanRaw().isEmpty()) << "scan blob did not round-trip";
+	EXPECT_EQ(1u, back->sensorData().cameraModels().size());
+
+	for(std::list<Signature *>::iterator iter = loaded.begin(); iter != loaded.end(); ++iter)
+	{
+		delete *iter;
+	}
+}
+
+TEST_F(DBDriverSqlite3Fixture, DBReaderReplaysWrittenDatabase)
+{
+	// Three consecutive nodes with odometry poses and neighbour links, i.e.
+	// the minimum a recorded session needs to be replayable.
+	const int kNodes = 3;
+	for(int i = 1; i <= kNodes; ++i)
+	{
+		const Transform pose(0.5f * i, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
+		Signature * s = new Signature(i, 0, 1, static_cast<double>(i), "", pose, Transform(),
+				makeRichData(i, static_cast<double>(i)));
+		if(i > 1)
+		{
+			const Transform motion(0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
+			s->addLink(Link(i, i - 1, Link::kNeighbor, motion.inverse()));
+		}
+		saveSignature(s);
+	}
+	// Close so the file is complete before DBReader opens it.
+	driver_->closeConnection(true);
+	delete driver_;
+	driver_ = nullptr;
+
+	std::unique_ptr<DBReader> reader(new DBReader(dbPath_, /*frameRate=*/0.0f,
+			/*odometryIgnored=*/false));
+	ASSERT_TRUE(reader->init()) << "DBReader could not open " << dbPath_;
+
+	int frames = 0;
+	Transform previousPose;
+	while(frames < kNodes + 1)
+	{
+		SensorCaptureInfo info;
+		SensorData data = reader->takeData(&info);
+		if(data.id() == 0 && data.imageRaw().empty() && data.imageCompressed().empty())
+		{
+			break;  // end of database
+		}
+		++frames;
+		EXPECT_FALSE(info.odomPose.isNull()) << "frame " << data.id() << " has no odometry pose";
+		if(!previousPose.isNull() && !info.odomPose.isNull())
+		{
+			// Poses were written 50 cm apart along x.
+			EXPECT_NEAR(previousPose.getDistance(info.odomPose), 0.5f, 1e-3f);
+		}
+		previousPose = info.odomPose;
+	}
+	EXPECT_EQ(kNodes, frames) << "DBReader returned " << frames << " of " << kNodes << " nodes";
+}
+
+// Only compressed buffers are persisted, so raw-only sensor data is stored with
+// empty payloads (documented on DBDriver::asyncSave()). Pinned here because it
+// is silent from the caller's point of view -- see makeRichData() above, which
+// compresses first.
+TEST_F(DBDriverSqlite3Fixture, RawOnlySensorDataIsNotPersisted)
+{
+	cv::Mat rgb(20, 20, CV_8UC3, cv::Scalar(10, 20, 30));
+	const CameraModel model(100.0, 100.0, 10.0, 10.0, Transform::getIdentity(), 0.0, cv::Size(20, 20));
+	SensorData raw(rgb, cv::Mat(), model, 42, 1.0);   // raw, never compressed
+	ASSERT_FALSE(raw.imageRaw().empty());
+	ASSERT_TRUE(raw.imageCompressed().empty());
+
+	saveSignature(new Signature(42, 0, 1, 1.0, "", Transform::getIdentity(), Transform(), raw));
+
+	std::list<Signature *> loaded;
+	driver_->loadSignatures(std::list<int>(1, 42), loaded);
+	ASSERT_EQ(1u, loaded.size());
+	driver_->loadNodeData(loaded);
+	loaded.front()->sensorData().uncompressData();
+	EXPECT_TRUE(loaded.front()->sensorData().imageRaw().empty())
+			<< "raw-only image unexpectedly survived a save/load round trip";
+	delete loaded.front();
+}
+
+// ---------------------------------------------------------------------------
+// Legacy schema round trips. Db/TargetVersion makes the driver create an older
+// schema, which is how a database recorded by an earlier rtabmap looks. The
+// readers for those layouts differ substantially -- e.g. link covariance was
+// stored as separate rotVariance/transVariance columns before the full
+// information matrix -- and none of it ran in the tests until now.
+// ---------------------------------------------------------------------------
+
+namespace {
+
+class DBSchemaVersionTest : public ::testing::TestWithParam<const char *>
+{
+protected:
+	void SetUp() override
+	{
+		dbPath_ = uniqueDbPath();
+		ParametersMap parameters;
+		parameters.insert(ParametersPair(Parameters::kDbTargetVersion(), GetParam()));
+		driver_ = new DBDriverSqlite3(parameters);
+		ASSERT_TRUE(driver_->openConnection(dbPath_, true))
+				<< "could not create a " << GetParam() << " database";
+	}
+
+	void TearDown() override
+	{
+		if(driver_)
+		{
+			driver_->closeConnection(false);
+			delete driver_;
+			driver_ = nullptr;
+		}
+		UFile::erase(dbPath_.c_str());
+	}
+
+	void saveSignature(Signature * s)
+	{
+		driver_->asyncSave(s);
+		driver_->emptyTrashes(false);
+	}
+
+	std::string dbPath_;
+	DBDriverSqlite3 * driver_ = nullptr;
+};
+
+}  // namespace
+
+TEST_P(DBSchemaVersionTest, NodesAndLinksSurviveARoundTrip)
+{
+	// Two nodes joined by a neighbour link carrying a non-default information
+	// matrix: older schemas store that as rotVariance/transVariance, newer ones
+	// as the full 6x6, so this exercises whichever reader the version needs.
+	cv::Mat info = cv::Mat::eye(6, 6, CV_64FC1);
+	info.at<double>(0,0) = info.at<double>(1,1) = info.at<double>(2,2) = 4.0;   // 1/transVariance
+	info.at<double>(3,3) = info.at<double>(4,4) = info.at<double>(5,5) = 100.0; // 1/rotVariance
+
+	const Transform motion(0.5f, 0.1f, 0.0f, 0.0f, 0.0f, 0.2f);
+	for(int id = 1; id <= 2; ++id)
+	{
+		const Transform pose(0.5f * (id - 1), 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
+		Signature * s = new Signature(id, 0, 1, static_cast<double>(id), "", pose, Transform(),
+				makeRichData(id, static_cast<double>(id)));
+		if(id == 2)
+		{
+			s->addLink(Link(2, 1, Link::kNeighbor, motion.inverse(), info));
+		}
+		saveSignature(s);
+	}
+
+	EXPECT_FALSE(driver_->getDatabaseVersion().empty());
+
+	std::list<Signature *> loaded;
+	driver_->loadSignatures(std::list<int>{1, 2}, loaded);
+	ASSERT_EQ(2u, loaded.size()) << "nodes did not survive the round trip";
+
+	// Payloads
+	driver_->loadNodeData(loaded);
+	for(Signature * s : loaded)
+	{
+		s->sensorData().uncompressData();
+		EXPECT_FALSE(s->sensorData().imageRaw().empty()) << "node " << s->id() << " lost its image";
+		EXPECT_EQ(1u, s->sensorData().cameraModels().size());
+	}
+
+	// Links: the second node must still point back at the first, with the
+	// variances recovered from whatever columns this schema uses.
+	std::multimap<int, Link> links;
+	driver_->loadLinks(2, links);
+	ASSERT_FALSE(links.empty()) << "link did not survive the round trip";
+	const Link & link = links.begin()->second;
+	EXPECT_EQ(1, link.to());
+	EXPECT_LT(link.transform().getDistance(motion.inverse()), 1e-3f);
+	EXPECT_NEAR(4.0, link.infMatrix().at<double>(0,0), 1e-6) << "translational variance lost";
+	EXPECT_NEAR(100.0, link.infMatrix().at<double>(3,3), 1e-6) << "rotational variance lost";
+
+	for(Signature * s : loaded)
+	{
+		delete s;
+	}
+}
+
+INSTANTIATE_TEST_SUITE_P(
+		Schemas,
+		DBSchemaVersionTest,
+		::testing::Values("0.16", "0.17", "0.18", "0.20", "0.22"),
+		[](const ::testing::TestParamInfo<const char *> & info) {
+			std::string name(info.param);
+			for(size_t i = 0; i < name.size(); ++i)
+			{
+				if(!isalnum(name[i])) name[i] = '_';
+			}
+			return "v" + name;
+		});

corelib/test/test_globalmap.cpp

@@ -1,7 +1,9 @@
 #include <gtest/gtest.h>
 #include <rtabmap/core/GlobalMap.h>
+#include <rtabmap/core/global_map/OccupancyGrid.h>
 #include <rtabmap/core/LocalGrid.h>
 #include <rtabmap/core/Parameters.h>
+#include <rtabmap/utilite/UConversion.h>
 #include <opencv2/core.hpp>
 
 using namespace rtabmap;
@@ -218,3 +220,112 @@ TEST(GlobalMapTest, GetMemoryUsed)
 	ASSERT_TRUE(map.update(poses));
 	EXPECT_GT(map.getMemoryUsed(), 0u);
 }
+
+// ---------------------------------------------------------------------------
+// The tests above drive GlobalMap through MockGlobalMap, which overrides
+// assemble(), so the real rasterisation never runs. These use the concrete
+// OccupancyGrid: cached local grids are assembled at known poses and the
+// resulting map is checked for extent and content.
+// ---------------------------------------------------------------------------
+
+namespace {
+
+// A local grid shaped like a short wall: `cols` obstacle cells one cell apart
+// along +x, plus a matching strip of empty cells in front of it.
+void addWallGridToCache(LocalGridCache & cache, int nodeId, int cols, float cellSize)
+{
+	cv::Mat obstacles(1, cols, CV_32FC2);
+	cv::Mat empty(1, cols, CV_32FC2);
+	for(int i = 0; i < cols; ++i)
+	{
+		obstacles.at<cv::Vec2f>(0, i) = cv::Vec2f(cellSize * i, 0.0f);
+		empty.at<cv::Vec2f>(0, i)     = cv::Vec2f(cellSize * i, -cellSize);
+	}
+	cache.add(nodeId, cv::Mat(), obstacles, empty, cellSize);
+}
+
+}  // namespace
+
+TEST(OccupancyGridTest, AssemblesCachedGridIntoMap)
+{
+	const float cellSize = 0.2f;
+	LocalGridCache cache;
+	addWallGridToCache(cache, 1, /*cols=*/5, cellSize);
+
+	ParametersMap parameters;
+	parameters.insert(ParametersPair(Parameters::kGridCellSize(), uNumber2Str(cellSize)));
+	OccupancyGrid grid(&cache, parameters);
+
+	std::map<int, Transform> poses;
+	poses.insert(std::make_pair(1, Transform::getIdentity()));
+	EXPECT_TRUE(grid.update(poses));
+
+	float xMin = 0.0f, yMin = 0.0f;
+	const cv::Mat map = grid.getMap(xMin, yMin);
+	ASSERT_FALSE(map.empty()) << "assemble() produced no map";
+	EXPECT_EQ(CV_8SC1, map.type()) << "occupancy maps are signed char (-1 unknown, 0 empty, 100 occupied)";
+	EXPECT_GT(map.total(), 5u) << "map smaller than the grid that was assembled";
+
+	// The wall spans 5 cells at 20 cm, so the map must be at least that wide.
+	EXPECT_GE(map.cols * cellSize, 5 * cellSize - 1e-3f);
+
+	int occupied = 0, empty = 0, unknown = 0;
+	for(int y = 0; y < map.rows; ++y)
+	{
+		for(int x = 0; x < map.cols; ++x)
+		{
+			const signed char v = map.at<signed char>(y, x);
+			if(v == 0)        ++empty;
+			else if(v > 0)    ++occupied;
+			else              ++unknown;
+		}
+	}
+	EXPECT_GT(occupied, 0) << "no occupied cells rasterised";
+	EXPECT_GT(empty, 0)    << "no empty cells rasterised";
+	EXPECT_EQ(map.total(), static_cast<size_t>(occupied + empty + unknown));
+}
+
+// Two nodes offset along x must produce a map wider than either alone: this is
+// what distinguishes real assembly from returning the last grid.
+TEST(OccupancyGridTest, SecondPoseExtendsTheMap)
+{
+	const float cellSize = 0.2f;
+	LocalGridCache cache;
+	addWallGridToCache(cache, 1, /*cols=*/5, cellSize);
+	addWallGridToCache(cache, 2, /*cols=*/5, cellSize);
+
+	ParametersMap parameters;
+	parameters.insert(ParametersPair(Parameters::kGridCellSize(), uNumber2Str(cellSize)));
+
+	OccupancyGrid single(&cache, parameters);
+	std::map<int, Transform> onePose;
+	onePose.insert(std::make_pair(1, Transform::getIdentity()));
+	single.update(onePose);
+	float xMin1 = 0.0f, yMin1 = 0.0f;
+	const cv::Mat mapOne = single.getMap(xMin1, yMin1);
+	ASSERT_FALSE(mapOne.empty());
+
+	OccupancyGrid both(&cache, parameters);
+	std::map<int, Transform> twoPoses;
+	twoPoses.insert(std::make_pair(1, Transform::getIdentity()));
+	twoPoses.insert(std::make_pair(2, Transform(2.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f)));
+	both.update(twoPoses);
+	float xMin2 = 0.0f, yMin2 = 0.0f;
+	const cv::Mat mapTwo = both.getMap(xMin2, yMin2);
+	ASSERT_FALSE(mapTwo.empty());
+
+	EXPECT_GT(mapTwo.cols, mapOne.cols)
+			<< "a node 2 m away did not widen the map (" << mapOne.cols << " -> " << mapTwo.cols << ")";
+	EXPECT_GT(mapTwo.total(), mapOne.total());
+}
+
+// An empty cache means nothing to assemble; the map must stay empty rather than
+// producing a degenerate one.
+TEST(OccupancyGridTest, EmptyCacheProducesEmptyMap)
+{
+	LocalGridCache cache;
+	OccupancyGrid grid(&cache, globalMapTestParams());
+	grid.update(std::map<int, Transform>());
+	float xMin = 0.0f, yMin = 0.0f;
+	EXPECT_TRUE(grid.getMap(xMin, yMin).empty());
+}

corelib/test/test_memory.cpp

@@ -7,6 +7,8 @@
 #include <rtabmap/core/Memory.h>
 #include <rtabmap/core/Link.h>
 #include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/RegistrationInfo.h>
+#include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/SensorData.h>
 #include <rtabmap/core/Signature.h>
 #include <rtabmap/core/Transform.h>
@@ -4015,3 +4017,146 @@ TEST(MemoryTest, CleanupLocalGridsFiltersObstaclesAgainstMap)
 	}
 	UFile::erase(dbPath.c_str());
 }
+
+// ---------------------------------------------------------------------------
+// Memory::computeIcpTransformMulti(): proximity refinement between two nodes
+// using their laser scans. Until now this only ran during the end-to-end
+// replays, which need the fetched sample databases.
+//
+// A corner is used so ICP has a unique optimum (a single line or plane would
+// leave a direction unobservable, making the result depend on the guess).
+// ---------------------------------------------------------------------------
+
+namespace {
+
+// 2D corner: floor at y=-half, wall at x=-half, in the sensor frame.
+LaserScan memoryCorner2D(float length = 4.0f, int pointsPerLine = 300, uint64_t seed = 0xBEEF)
+{
+	cv::RNG rng(seed);
+	cv::Mat data(1, 2 * pointsPerLine, CV_32FC2);
+	const float half = 0.5f * length;
+	int idx = 0;
+	for(int i = 0; i < pointsPerLine; ++i, ++idx)
+	{
+		data.at<cv::Vec2f>(0, idx) = cv::Vec2f(
+				rng.uniform(-half, half), -half + rng.uniform(-0.005f, 0.005f));
+	}
+	for(int i = 0; i < pointsPerLine; ++i, ++idx)
+	{
+		data.at<cv::Vec2f>(0, idx) = cv::Vec2f(
+				-half + rng.uniform(-0.005f, 0.005f), rng.uniform(-half, half));
+	}
+	return LaserScan(data, /*maxPoints=*/0, /*maxRange=*/0.0f, LaserScan::kXY);
+}
+
+ParametersMap icpMemoryParams()
+{
+	ParametersMap p = defaultMemoryParams();
+	p[Parameters::kRegStrategy()]           = "1";     // ICP
+	p[Parameters::kRegForce3DoF()]          = "true";
+	p[Parameters::kMemBinDataKept()]        = "true";  // keep the scans
+	p[Parameters::kIcpPointToPlane()]       = "false";
+	p[Parameters::kIcpVoxelSize()]          = "0.0";
+	p[Parameters::kIcpCorrespondenceRatio()]= "0.1";
+	p[Parameters::kMemLaserScanVoxelSize()] = "0.0";
+	return p;
+}
+
+}  // namespace
+
+TEST_F(MemoryFixture, ComputeIcpTransformMultiAlignsCornerScans)
+{
+	reinit(icpMemoryParams());
+
+	// Same corner seen from two viewpoints 12 cm / 2.9 deg apart.
+	const Transform motion(0.10f, 0.06f, 0.0f, 0.0f, 0.0f, 0.05f);
+	const LaserScan corner = memoryCorner2D();
+
+	SensorData first(image_);
+	first.setLaserScan(corner);
+	ASSERT_TRUE(memory_->update(first, Transform::getIdentity(), covariance_));
+	const int oldId = memory_->getLastSignatureId();
+
+	SensorData second(image_);
+	second.setLaserScan(util3d::transformLaserScan(corner, motion.inverse()));
+	ASSERT_TRUE(memory_->update(second, motion, covariance_));
+	const int newId = memory_->getLastSignatureId();
+	ASSERT_NE(oldId, newId);
+
+	std::map<int, Transform> poses;
+	poses.insert(std::make_pair(oldId, Transform::getIdentity()));
+	poses.insert(std::make_pair(newId, motion));
+
+	RegistrationInfo info;
+	const Transform t = memory_->computeIcpTransformMulti(newId, oldId, poses, &info);
+	ASSERT_FALSE(t.isNull()) << "ICP refinement failed: " << info.rejectedMsg;
+
+	// computeIcpTransformMulti(newId, oldId, ...) reports the transform from the
+	// new node to the old one, i.e. the inverse of the motion between them.
+	const Transform expected = motion.inverse();
+	EXPECT_LT(t.getDistance(expected), 0.002f)
+			<< "got " << t.prettyPrint() << " expected " << expected.prettyPrint();
+	const float angDeg = t.getAngle(expected) * 180.0f / static_cast<float>(CV_PI);
+	EXPECT_LT(angDeg, 0.2f) << "rotation off by " << angDeg << " deg";
+	EXPECT_GT(info.icpInliersRatio, 0.5f) << "poor overlap for two views of the same corner";
+}
+
+// Without scans there is nothing for ICP to align: the refinement must decline
+// with a reason rather than returning a bogus transform.
+TEST_F(MemoryFixture, ComputeIcpTransformMultiRejectsNodesWithoutScans)
+{
+	reinit(icpMemoryParams());
+	ASSERT_TRUE(update());
+	const int oldId = memory_->getLastSignatureId();
+	ASSERT_TRUE(update());
+	const int newId = memory_->getLastSignatureId();
+
+	std::map<int, Transform> poses;
+	poses.insert(std::make_pair(oldId, Transform::getIdentity()));
+	poses.insert(std::make_pair(newId, Transform(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f)));
+
+	RegistrationInfo info;
+	const Transform t = memory_->computeIcpTransformMulti(newId, oldId, poses, &info);
+	EXPECT_TRUE(t.isNull()) << "expected refusal, got " << t.prettyPrint();
+	EXPECT_FALSE(info.rejectedMsg.empty())
+			<< "declined without a reason, so a caller cannot tell 'no scans' from "
+			   "a genuine registration failure";
+	EXPECT_LE(info.icpInliersRatio, 0.0f);
+}
+
+// The registration is also declined when the guess puts the two viewpoints
+// beyond their combined sensor range, since the scans then cannot overlap. That
+// early-out reports its own reason, so a caller can tell it apart from a
+// failed alignment.
+TEST_F(MemoryFixture, ComputeIcpTransformMultiRejectsScansTooFarApart)
+{
+	reinit(icpMemoryParams());
+
+	// Same corner, but with a declared 2 m max range.
+	const LaserScan corner = memoryCorner2D();
+	const LaserScan ranged(corner.data(), corner.maxPoints(), /*rangeMax=*/2.0f, corner.format());
+	ASSERT_GT(ranged.rangeMax(), 0.0f);
+
+	SensorData first(image_);
+	first.setLaserScan(ranged);
+	ASSERT_TRUE(memory_->update(first, Transform::getIdentity(), covariance_));
+	const int oldId = memory_->getLastSignatureId();
+
+	SensorData second(image_);
+	second.setLaserScan(ranged);
+	const Transform farAway(20.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
+	ASSERT_TRUE(memory_->update(second, farAway, covariance_));
+	const int newId = memory_->getLastSignatureId();
+
+	// 20 m apart with 2 m + 2 m of range: no overlap is possible.
+	std::map<int, Transform> poses;
+	poses.insert(std::make_pair(oldId, Transform::getIdentity()));
+	poses.insert(std::make_pair(newId, farAway));
+
+	RegistrationInfo info;
+	const Transform t = memory_->computeIcpTransformMulti(newId, oldId, poses, &info);
+	EXPECT_TRUE(t.isNull()) << "aligned scans that cannot overlap: " << t.prettyPrint();
+	ASSERT_FALSE(info.rejectedMsg.empty()) << "declined without a reason";
+	EXPECT_NE(info.rejectedMsg.find("Too far"), std::string::npos)
+			<< "unexpected reason: " << info.rejectedMsg;
+}

corelib/test/test_odometry.cpp

@@ -2,8 +2,16 @@
 #include <rtabmap/core/Odometry.h>
 #include <rtabmap/core/OdometryInfo.h>
 #include <rtabmap/core/CameraModel.h>
+#include <rtabmap/core/StereoCameraModel.h>
+#include <rtabmap/core/SensorData.h>
+#include <rtabmap/core/LaserScan.h>
+#include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/Parameters.h>
+#include <rtabmap/utilite/UConversion.h>
 #include <opencv2/core.hpp>
+#include <opencv2/imgcodecs.hpp>
+#include <memory>
+#include <string>
 
 using namespace rtabmap;
 
@@ -179,3 +187,359 @@ TEST(OdometryTest, DefaultCapabilityFlags)
 	EXPECT_FALSE(odometry.canProcessRawImages());
 	EXPECT_FALSE(odometry.canProcessAsyncIMU());
 }
+
+// ---------------------------------------------------------------------------
+// Strategies driven through the public API on real frames committed under
+// data/. The tests above use MockOdometry, which overrides computeTransform(),
+// so they exercise the base class but never enter a backend implementation.
+// These run the real strategies end to end -- feature extraction,
+// correspondence and motion estimation all execute. Parameterized on
+// Odom/Strategy, so covering another backend is one list entry.
+// ---------------------------------------------------------------------------
+
+namespace {
+
+const char * strategyName(Odometry::Type type)
+{
+	switch(type)
+	{
+	case Odometry::kTypeF2M: return "F2M";
+	case Odometry::kTypeF2F: return "F2F";
+	default:                 return "other";
+	}
+}
+
+// data/stereo_rect holds two rectified pairs (50 and 60) plus the calibration:
+// enough for one initialisation frame and one motion estimate.
+SensorData loadStereoFrame(const std::string & name, int id, double stamp)
+{
+	const std::string root(RTABMAP_TEST_DATA_ROOT);
+	const cv::Mat left  = cv::imread(root + "/stereo_rect/left/"  + name + ".jpg", cv::IMREAD_GRAYSCALE);
+	const cv::Mat right = cv::imread(root + "/stereo_rect/right/" + name + ".jpg", cv::IMREAD_GRAYSCALE);
+	StereoCameraModel model;
+	if(left.empty() || right.empty() || !model.load(root + "/stereo_rect", "stereo"))
+	{
+		return SensorData();
+	}
+	return SensorData(left, right, model, id, stamp);
+}
+
+// data/rgbd holds two RGB-D frames (17 and 154) with per-frame calibration.
+SensorData loadRgbdFrame(const std::string & name, int id, double stamp)
+{
+	const std::string root(RTABMAP_TEST_DATA_ROOT);
+	const cv::Mat rgb   = cv::imread(root + "/rgbd/rgb/"   + name + ".jpg", cv::IMREAD_COLOR);
+	const cv::Mat depth = cv::imread(root + "/rgbd/depth/" + name + ".png", cv::IMREAD_UNCHANGED);
+	CameraModel model;
+	if(rgb.empty() || depth.empty() || !model.load(root + "/rgbd/calib", name))
+	{
+		return SensorData();
+	}
+	return SensorData(rgb, depth, model, id, stamp);
+}
+
+class OdometryStrategyTest : public ::testing::TestWithParam<Odometry::Type>
+{
+protected:
+	// Deliberately minimal: exercise each strategy's default path rather than
+	// a tuned configuration.
+	std::unique_ptr<Odometry> createOdometry() const
+	{
+		ParametersMap parameters;
+		parameters.insert(ParametersPair(Parameters::kOdomStrategy(),
+				uNumber2Str(static_cast<int>(GetParam()))));
+		std::unique_ptr<Odometry> odometry(Odometry::create(parameters));
+		if(odometry && odometry->getType() != GetParam())
+		{
+			// create() falls back to F2M for a backend that is not compiled
+			// in, which would otherwise silently test F2M twice.
+			return nullptr;
+		}
+		return odometry;
+	}
+};
+
+}  // namespace
+
+// A first frame has nothing to register against: every strategy should accept
+// it, report the identity pose, and initialise its map / previous frame.
+TEST_P(OdometryStrategyTest, FirstStereoFrameInitialisesAtOrigin)
+{
+	std::unique_ptr<Odometry> odometry = createOdometry();
+	if(!odometry)
+	{
+		GTEST_SKIP() << strategyName(GetParam()) << " not built in";
+	}
+
+	SensorData first = loadStereoFrame("50", 1, 0.0);
+	ASSERT_FALSE(first.imageRaw().empty()) << "data/stereo_rect/left/50.jpg missing";
+
+	OdometryInfo info;
+	const Transform pose = odometry->process(first, &info);
+
+	EXPECT_FALSE(pose.isNull());
+	EXPECT_TRUE(pose.isIdentity()) << "first pose should be the origin, got " << pose.prettyPrint();
+	EXPECT_EQ(0, info.reg.inliers) << "nothing to register against on the first frame";
+}
+
+// Second frame: the strategy must recover a real motion between the two
+// rectified pairs. The bounds are loose on purpose -- the claim is "a
+// plausible, non-degenerate transform from real correspondences", not a
+// specific value, which differs per backend.
+TEST_P(OdometryStrategyTest, StereoPairRecoversMotion)
+{
+	std::unique_ptr<Odometry> odometry = createOdometry();
+	if(!odometry)
+	{
+		GTEST_SKIP() << strategyName(GetParam()) << " not built in";
+	}
+
+	SensorData first  = loadStereoFrame("50", 1, 0.0);
+	SensorData second = loadStereoFrame("60", 2, 0.1);
+	ASSERT_FALSE(first.imageRaw().empty());
+	ASSERT_FALSE(second.imageRaw().empty());
+
+	OdometryInfo firstInfo;
+	ASSERT_FALSE(odometry->process(first, &firstInfo).isNull());
+
+	OdometryInfo info;
+	const Transform pose = odometry->process(second, &info);
+
+	ASSERT_FALSE(pose.isNull())
+			<< strategyName(GetParam()) << " lost tracking between frames 50 and 60";
+	EXPECT_GT(info.reg.inliers, 20)
+			<< strategyName(GetParam()) << " too few inliers (matches=" << info.reg.matches << ")";
+	EXPECT_LE(info.reg.inliers, info.reg.matches);
+
+	// The frames are ~15 cm apart; beyond a metre the estimate diverged.
+	const float distance = pose.getNorm();
+	EXPECT_GT(distance, 0.01f) << strategyName(GetParam()) << " reported no motion at all";
+	EXPECT_LT(distance, 1.0f)  << strategyName(GetParam()) << " implausible motion "
+			<< pose.prettyPrint();
+}
+
+// Same entry point with RGB-D input, so the depth-to-3D path runs instead of
+// stereo correspondence. Frames 17 and 154 are far apart in the sequence, so
+// losing tracking is a legitimate outcome; what must hold is that the reported
+// info agrees with the returned transform.
+TEST_P(OdometryStrategyTest, HandlesRgbdFrames)
+{
+	std::unique_ptr<Odometry> odometry = createOdometry();
+	if(!odometry)
+	{
+		GTEST_SKIP() << strategyName(GetParam()) << " not built in";
+	}
+
+	SensorData first  = loadRgbdFrame("17", 1, 0.0);
+	SensorData second = loadRgbdFrame("154", 2, 0.1);
+	ASSERT_FALSE(first.imageRaw().empty())  << "data/rgbd/rgb/17.jpg missing";
+	ASSERT_FALSE(second.imageRaw().empty()) << "data/rgbd/rgb/154.jpg missing";
+
+	OdometryInfo firstInfo;
+	EXPECT_FALSE(odometry->process(first, &firstInfo).isNull());
+
+	OdometryInfo info;
+	const Transform pose = odometry->process(second, &info);
+	if(pose.isNull())
+	{
+		EXPECT_LT(info.reg.inliers, 20) << "null transform but plenty of inliers";
+	}
+	else
+	{
+		EXPECT_GT(info.reg.inliers, 0);
+		EXPECT_LT(pose.getNorm(), 20.0f) << "implausible jump " << pose.prettyPrint();
+	}
+}
+
+INSTANTIATE_TEST_SUITE_P(
+		Strategies,
+		OdometryStrategyTest,
+		::testing::Values(Odometry::kTypeF2M, Odometry::kTypeF2F),
+		[](const ::testing::TestParamInfo<Odometry::Type> & info) {
+			return strategyName(info.param);
+		});
+
+// ---------------------------------------------------------------------------
+// Scan-matching odometry (Reg/Strategy=1). The visual tests above depend on
+// real imagery; these use synthetic geometry instead, which lets the expected
+// motion be known exactly rather than merely plausible.
+//
+// A corner is used on purpose: two perpendicular surfaces constrain every
+// translational DoF, so ICP has a unique optimum. A single plane or a corridor
+// would leave a direction unobservable and the test would pass or fail on the
+// initial guess rather than on the registration.
+// ---------------------------------------------------------------------------
+
+namespace {
+
+// 2D corner: floor at y=-half and wall at x=-half, expressed in the sensor
+// frame of the first observation. Jitter keeps the per-point normals
+// well-defined (see the same construction in test_registrationicp.cpp).
+LaserScan makeCorner2D(float length = 4.0f, int pointsPerLine = 400, uint64_t seed = 0xC0FFEE)
+{
+	cv::RNG rng(seed);
+	cv::Mat data(1, 2 * pointsPerLine, CV_32FC2);
+	const float half = 0.5f * length;
+	int idx = 0;
+	for(int i = 0; i < pointsPerLine; ++i, ++idx)
+	{
+		data.at<cv::Vec2f>(0, idx) = cv::Vec2f(
+				rng.uniform(-half, half), -half + rng.uniform(-0.005f, 0.005f));
+	}
+	for(int i = 0; i < pointsPerLine; ++i, ++idx)
+	{
+		data.at<cv::Vec2f>(0, idx) = cv::Vec2f(
+				-half + rng.uniform(-0.005f, 0.005f), rng.uniform(-half, half));
+	}
+	return LaserScan(data, /*maxPoints=*/0, /*maxRange=*/0.0f, LaserScan::kXY);
+}
+
+// 3D corner: floor plus two walls, so all 6 DoF are constrained.
+LaserScan makeCorner3D(float length = 4.0f, int pointsPerSurface = 400, uint64_t seed = 0xC0FFEE)
+{
+	cv::RNG rng(seed);
+	cv::Mat data(1, 3 * pointsPerSurface, CV_32FC3);
+	const float half = 0.5f * length;
+	int idx = 0;
+	for(int i = 0; i < pointsPerSurface; ++i, ++idx)  // floor z=-half
+	{
+		data.at<cv::Vec3f>(0, idx) = cv::Vec3f(rng.uniform(-half, half), rng.uniform(-half, half),
+				-half + static_cast<float>(rng.gaussian(0.005)));
+	}
+	for(int i = 0; i < pointsPerSurface; ++i, ++idx)  // wall x=-half
+	{
+		data.at<cv::Vec3f>(0, idx) = cv::Vec3f(-half + static_cast<float>(rng.gaussian(0.005)),
+				rng.uniform(-half, half), rng.uniform(-half, half));
+	}
+	for(int i = 0; i < pointsPerSurface; ++i, ++idx)  // wall y=-half
+	{
+		data.at<cv::Vec3f>(0, idx) = cv::Vec3f(rng.uniform(-half, half),
+				-half + static_cast<float>(rng.gaussian(0.005)), rng.uniform(-half, half));
+	}
+	return LaserScan(data, /*maxPoints=*/0, /*maxRange=*/0.0f, LaserScan::kXYZ);
+}
+
+SensorData makeScanData(const LaserScan & scan, int id, double stamp)
+{
+	SensorData data;
+	data.setId(id);
+	data.setStamp(stamp);
+	data.setLaserScan(scan);
+	return data;
+}
+
+ParametersMap icpOdometryParameters(Odometry::Type type, bool force3DoF, bool guessMotion)
+{
+	ParametersMap parameters;
+	parameters.insert(ParametersPair(Parameters::kOdomStrategy(), uNumber2Str(static_cast<int>(type))));
+	parameters.insert(ParametersPair(Parameters::kRegStrategy(), "1"));  // ICP
+	parameters.insert(ParametersPair(Parameters::kRegForce3DoF(), force3DoF ? "true" : "false"));
+	parameters.insert(ParametersPair(Parameters::kOdomGuessMotion(), guessMotion ? "true" : "false"));
+	parameters.insert(ParametersPair(Parameters::kIcpPointToPlane(), "false"));
+	parameters.insert(ParametersPair(Parameters::kIcpVoxelSize(), "0.0"));
+	parameters.insert(ParametersPair(Parameters::kIcpCorrespondenceRatio(), "0.1"));
+	return parameters;
+}
+
+// Distance between two transforms, reported as translation and angle so a
+// failure says which part diverged.
+void expectPoseNear(const Transform & actual, const Transform & expected,
+		float transTol, float angTolDeg, const std::string & what)
+{
+	ASSERT_FALSE(actual.isNull()) << what << ": null transform";
+	EXPECT_LT(actual.getDistance(expected), transTol)
+			<< what << ": translation off -- got " << actual.prettyPrint()
+			<< " expected " << expected.prettyPrint();
+	const float angDeg = actual.getAngle(expected) * 180.0f / static_cast<float>(CV_PI);
+	EXPECT_LT(angDeg, angTolDeg)
+			<< what << ": rotation off by " << angDeg << " deg -- got " << actual.prettyPrint()
+			<< " expected " << expected.prettyPrint();
+}
+
+// Observe one corner from two viewpoints and return what odometry reports for
+// the second frame. `guess` may be null (no external guess).
+Transform runTwoScanOdometry(
+		const ParametersMap & parameters,
+		const LaserScan & corner,
+		const Transform & motion,
+		const Transform & guess,
+		OdometryInfo * info)
+{
+	std::unique_ptr<Odometry> odometry(Odometry::create(parameters));
+	if(!odometry)
+	{
+		return Transform();
+	}
+	// The corner is fixed in the world; the second scan is the same points
+	// expressed in the moved sensor frame.
+	SensorData first  = makeScanData(corner, 1, 0.0);
+	SensorData second = makeScanData(util3d::transformLaserScan(corner, motion.inverse()), 2, 0.1);
+
+	OdometryInfo firstInfo;
+	const Transform firstPose = odometry->process(first, &firstInfo);
+	if(firstPose.isNull())
+	{
+		return Transform();
+	}
+	return guess.isNull() ? odometry->process(second, info)
+	                      : odometry->process(second, guess, info);
+}
+
+}  // namespace
+
+// 2D corner, no guess: ICP starts from identity and must find the motion from
+// the geometry alone.
+TEST_P(OdometryStrategyTest, Icp2DCornerRecoversMotionWithoutGuess)
+{
+	const Transform motion(0.10f, 0.06f, 0.0f, 0.0f, 0.0f, 0.05f);  // 12 cm / ~3 deg
+	OdometryInfo info;
+	const Transform pose = runTwoScanOdometry(
+			icpOdometryParameters(GetParam(), /*force3DoF=*/true, /*guessMotion=*/false),
+			makeCorner2D(), motion, Transform(), &info);
+	if(pose.isNull() && info.reg.icpInliersRatio == 0.0f)
+	{
+		GTEST_SKIP() << "scan-matching odometry unavailable for this strategy";
+	}
+	expectPoseNear(pose, motion, 0.002f, 0.2f, "2D corner, no guess");
+}
+
+// 3D corner, no guess: same in 6DoF, so the two walls plus floor have to pin
+// all three rotations as well.
+TEST_P(OdometryStrategyTest, Icp3DCornerRecoversMotionWithoutGuess)
+{
+	const Transform motion(0.10f, 0.06f, 0.04f, 0.02f, 0.03f, 0.05f);
+	OdometryInfo info;
+	const Transform pose = runTwoScanOdometry(
+			icpOdometryParameters(GetParam(), /*force3DoF=*/false, /*guessMotion=*/false),
+			makeCorner3D(), motion, Transform(), &info);
+	if(pose.isNull() && info.reg.icpInliersRatio == 0.0f)
+	{
+		GTEST_SKIP() << "scan-matching odometry unavailable for this strategy";
+	}
+	expectPoseNear(pose, motion, 0.002f, 0.2f, "3D corner, no guess");
+}
+
+// With a guess that is deliberately off, the result must be closer to the truth
+// than the guess was -- i.e. ICP actually converged instead of returning the
+// guess unchanged, which is the failure mode this asserts against.
+TEST_P(OdometryStrategyTest, IcpConvergesFromOffsetGuess)
+{
+	const Transform motion(0.10f, 0.06f, 0.0f, 0.0f, 0.0f, 0.05f);
+	// Guess is in the right neighbourhood but ~4 cm and ~1.7 deg away.
+	const Transform guess(0.14f, 0.03f, 0.0f, 0.0f, 0.0f, 0.02f);
+	const float guessError = guess.getDistance(motion);
+
+	OdometryInfo info;
+	const Transform pose = runTwoScanOdometry(
+			icpOdometryParameters(GetParam(), /*force3DoF=*/true, /*guessMotion=*/false),
+			makeCorner2D(), motion, guess, &info);
+	if(pose.isNull() && info.reg.icpInliersRatio == 0.0f)
+	{
+		GTEST_SKIP() << "scan-matching odometry unavailable for this strategy";
+	}
+	ASSERT_FALSE(pose.isNull()) << "lost tracking despite a close guess";
+	EXPECT_LT(pose.getDistance(motion), guessError)
+			<< "result (" << pose.prettyPrint() << ") is no closer to the truth than the guess ("
+			<< guess.prettyPrint() << "); ICP did not converge";
+	expectPoseNear(pose, motion, 0.002f, 0.2f, "2D corner, offset guess");
+}

corelib/test/test_optimizer.cpp

@@ -13,6 +13,7 @@
 // them rather than backend-specific quirks.
 
 #include <gtest/gtest.h>
+#include <rtabmap/core/Version.h>
 #include <rtabmap/core/Optimizer.h>
 #include <rtabmap/core/Link.h>
 #include <rtabmap/core/Parameters.h>
@@ -870,6 +871,14 @@ protected:
 		{
 			GTEST_SKIP() << optimizerTypeName(t) << " not built in";
 		}
+#ifndef RTABMAP_VERTIGO
+		if(std::get<1>(GetParam()))
+		{
+			// Without Vertigo, optimize() warns and silently clears the robust
+			// flag, so the corrupted loop closure is never rejected.
+			GTEST_SKIP() << "robust optimization needs Vertigo (WITH_VERTIGO=ON)";
+		}
+#endif
 	}
 };
 

corelib/test/test_rtabmap.cpp

@@ -3972,3 +3972,57 @@ TEST(RtabmapTest, GetGraphWithSignaturePayloadsAttachesRequestedFields)
 	rtabmap.close(false);
 }
 
+
+// ---------------------------------------------------------------------------
+// Ground-truth statistics. Rtabmap/ComputeRMSE is on by default, but the block
+// that computes it only runs when the memory holds ground-truth poses, which
+// requires SensorData::setGroundTruth() -- something no test did, so the whole
+// RMSE-vs-ground-truth path only ran during the end-to-end replays.
+// ---------------------------------------------------------------------------
+
+TEST_F(RtabmapFixture, ComputesGroundTruthStatisticsWhenGroundTruthProvided)
+{
+	ParametersMap parameters = defaultRtabmapParams();
+	parameters[Parameters::kRtabmapComputeRMSE()] = "true";
+	reinit(parameters);
+
+	// More than 5 matched poses on purpose: graph::calcRMSE() only aligns the
+	// trajectories with an SVD fit above that count, and falls back to anchoring
+	// on the first pose below it -- so a shorter run would exercise a degenerate
+	// path and report a less meaningful error.
+	//
+	// Odometry drifts 5 cm per frame relative to the ground truth, so the RMSE
+	// must come out non-zero.
+	for(int i = 1; i <= 8; ++i)
+	{
+		SensorData data(image_);
+		data.setId(i);
+		const Transform truth(0.5f * i, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
+		const Transform odom (0.5f * i + 0.05f * i, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
+		data.setGroundTruth(truth);
+		ASSERT_TRUE(rtabmap_->process(data, odom, covariance_)) << "frame " << i << " rejected";
+	}
+
+	const std::map<std::string, float> & stats = rtabmap_->getStatistics().data();
+	ASSERT_TRUE(stats.find(Statistics::kGtTranslational_rmse()) != stats.end())
+			<< "no ground-truth RMSE statistic published";
+	const float rmse = stats.at(Statistics::kGtTranslational_rmse());
+	EXPECT_GT(rmse, 0.0f) << "ground truth differs from odometry, so RMSE cannot be zero";
+	EXPECT_LT(rmse, 1.0f) << "implausible RMSE " << rmse << " for a 20 cm drift";
+
+	// The companion statistics come from the same block.
+	EXPECT_TRUE(stats.find(Statistics::kGtTranslational_max()) != stats.end());
+	EXPECT_TRUE(stats.find(Statistics::kGtRotational_rmse()) != stats.end());
+	EXPECT_GE(stats.at(Statistics::kGtTranslational_max()), rmse)
+			<< "max error should be at least the RMSE";
+}
+
+// Without ground truth the same statistics must be absent rather than zero, so
+// a consumer can tell "not measured" from "perfect".
+TEST_F(RtabmapFixture, NoGroundTruthStatisticsWithoutGroundTruth)
+{
+	ASSERT_TRUE(process());
+	ASSERT_TRUE(process());
+	const std::map<std::string, float> & stats = rtabmap_->getStatistics().data();
+	EXPECT_TRUE(stats.find(Statistics::kGtTranslational_rmse()) == stats.end());
+}

corelib/test/test_rtabmap_integration.cpp

@@ -1234,13 +1234,17 @@ TEST_F(RtabmapIntegrationFixture, PR2_Scan2D_RGBD_IcpReg)
 	EXPECT_EQ(21, result.finalGlobalGraphSize);
 	EXPECT_GE(result.proximityDetections, 1)
 			<< "PR2 2D-scan dataset should produce proximity detections";
-	// Observed: empty 22785-24111, obstacle 1302-1637. Range widened to
+	// Cell counts vary run to run: the RANSAC correspondence rejector in the
-	// absorb run-to-run variance from the RANSAC correspondence rejector
+	// PCL ICP path (util3d_registration.cpp) shifts the registered scans
-	// installed in the PCL ICP path (util3d_registration.cpp).
+	// slightly, which moves how many cells they sweep. Observed empty
-	EXPECT_GE(result.gridEmptyCells, 22500);
+	// 22785-24111, obstacle 1302-1730. The previous caps sat ~4% above the
-	EXPECT_LE(result.gridEmptyCells, 24500);
+	// then-highest sample and were tripped by the next run, so these carry
-	EXPECT_GE(result.gridObstacleCells, 1250);
+	// ~10% headroom instead -- still far tighter than a broken grid (which
-	EXPECT_LE(result.gridObstacleCells, 1700);
+	// collapses toward 0 or changes by multiples), which is what they guard.
+	EXPECT_GE(result.gridEmptyCells, 21500);
+	EXPECT_LE(result.gridEmptyCells, 25500);
+	EXPECT_GE(result.gridObstacleCells, 1150);
+	EXPECT_LE(result.gridObstacleCells, 2000);
 #ifdef RTABMAP_OCTOMAP
 	// 2D-laser-only signatures: nothing to assemble into a 3D OctoMap.
 	EXPECT_EQ(0, result.octomapEmptyCells);
@@ -1684,6 +1688,15 @@ TEST_F(RtabmapIntegrationFixture, RobustGraphOptimizationStereo)
 			std::cerr << "[skip] " << v.label << " (optimizer unavailable)\n";
 			continue;
 		}
+#ifndef RTABMAP_VERTIGO
+		if(v.robust)
+		{
+			// optimize() would warn and clear the robust flag, so this variant
+			// would silently measure the non-robust path instead of failing.
+			std::cerr << "[skip] " << v.label << " (needs Vertigo)\n";
+			continue;
+		}
+#endif
 		SCOPED_TRACE(std::string(v.label));
 
 		ParametersMap params = baseRtabmapParams();

corelib/test/test_sensorcapture.cpp

@@ -444,4 +444,3 @@ TEST(SensorCaptureTest, VeryLowFrameRate)
 	EXPECT_GE(elapsed, 0.5);
 	EXPECT_LT(elapsed, 0.75); // allow scheduler jitter on loaded CI runners (e.g. macOS)
 }
-

utilite/test/test_ulogger.cpp

@@ -7,9 +7,24 @@
 #include <sstream>
 #include <thread>
 
-TEST(ULoggerTest, DefaultState)
+namespace {
+
+// The tests below set the logger's type/level as part of what they verify.
+// Restore the defaults afterwards (kTypeNoLog, kInfo, ...) so a test can never
+// leave the process logging and spill output into whatever runs next.
+class ULoggerTest : public ::testing::Test
+{
+protected:
+	void TearDown() override
+	{
+		ULogger::reset();
+	}
+};
+
+} // namespace
+
+TEST_F(ULoggerTest, DefaultState)
 {
-    ULogger::reset();
     
     EXPECT_EQ(ULogger::type(), ULogger::kTypeNoLog);
     EXPECT_EQ(ULogger::level(), ULogger::kInfo);
@@ -23,7 +38,7 @@ TEST(ULoggerTest, DefaultState)
     EXPECT_FALSE(ULogger::isBuffered());
 }
 
-TEST(ULoggerTest, SetType)
+TEST_F(ULoggerTest, SetType)
 {
     ULogger::setType(ULogger::kTypeConsole);
     EXPECT_EQ(ULogger::type(), ULogger::kTypeConsole);
@@ -38,7 +53,7 @@ TEST(ULoggerTest, SetType)
     UFile::erase("test_log.txt");
 }
 
-TEST(ULoggerTest, SetLevel)
+TEST_F(ULoggerTest, SetLevel)
 {
     ULogger::setLevel(ULogger::kDebug);
     EXPECT_EQ(ULogger::level(), ULogger::kDebug);
@@ -56,9 +71,8 @@ TEST(ULoggerTest, SetLevel)
     EXPECT_EQ(ULogger::level(), ULogger::kFatal);
 }
 
-TEST(ULoggerTest, LevelFiltering)
+TEST_F(ULoggerTest, LevelFiltering)
 {
-    ULogger::reset();
     std::string testFile = "logger_level_filtering_test.txt";
     ULogger::setType(ULogger::kTypeFile, testFile, false);
 
@@ -88,9 +102,8 @@ TEST(ULoggerTest, LevelFiltering)
     EXPECT_TRUE(uStrContains(fileContent, "ERROR"));
 }
 
-TEST(ULoggerTest, SetPrintTime)
+TEST_F(ULoggerTest, SetPrintTime)
 {
-    ULogger::reset();
 
     std::string testFile = "logger_print_time_test.txt";
     ULogger::setType(ULogger::kTypeFile, testFile, false);
@@ -119,9 +132,8 @@ TEST(ULoggerTest, SetPrintTime)
     file.close(); 
 }
 
-TEST(ULoggerTest, SetPrintLevel)
+TEST_F(ULoggerTest, SetPrintLevel)
 {
-    ULogger::reset();
 
     std::string testFile = "logger_print_level_test.txt";
     ULogger::setType(ULogger::kTypeFile, testFile, false);
@@ -150,9 +162,8 @@ TEST(ULoggerTest, SetPrintLevel)
     file.close(); 
 }
 
-TEST(ULoggerTest, SetPrintEndline)
+TEST_F(ULoggerTest, SetPrintEndline)
 {
-    ULogger::reset();
     std::string testFile = "logger_print_endline_test.txt";
     ULogger::setType(ULogger::kTypeFile, testFile, false);
     
@@ -179,9 +190,8 @@ TEST(ULoggerTest, SetPrintEndline)
     file.close(); 
 }
 
-TEST(ULoggerTest, SetPrintColored)
+TEST_F(ULoggerTest, SetPrintColored)
 {
-    ULogger::reset();
     
     ULogger::setPrintColored(false);
     EXPECT_FALSE(ULogger::isPrintColored());
@@ -190,9 +200,8 @@ TEST(ULoggerTest, SetPrintColored)
     EXPECT_TRUE(ULogger::isPrintColored());
 }
 
-TEST(ULoggerTest, SetPrintWhere)
+TEST_F(ULoggerTest, SetPrintWhere)
 {
-    ULogger::reset();
     std::string testFile = "logger_print_where_test.txt";
     ULogger::setType(ULogger::kTypeFile, testFile, false);
     
@@ -219,9 +228,8 @@ TEST(ULoggerTest, SetPrintWhere)
     file.close(); 
 }
 
-TEST(ULoggerTest, SetPrintWhereFullPath)
+TEST_F(ULoggerTest, SetPrintWhereFullPath)
 {
-    ULogger::reset();
     std::string testFile = "logger_print_where_full_test.txt";
     ULogger::setType(ULogger::kTypeFile, testFile, false);
     
@@ -248,9 +256,8 @@ TEST(ULoggerTest, SetPrintWhereFullPath)
     file.close(); 
 }
 
-TEST(ULoggerTest, SetPrintThreadId)
+TEST_F(ULoggerTest, SetPrintThreadId)
 {
-    ULogger::reset();
     std::string testFile = "logger_print_thread_id_test.txt";
     ULogger::setType(ULogger::kTypeFile, testFile, false);
     
@@ -277,9 +284,8 @@ TEST(ULoggerTest, SetPrintThreadId)
     file.close(); 
 }
 
-TEST(ULoggerTest, SetBuffered)
+TEST_F(ULoggerTest, SetBuffered)
 {
-    ULogger::reset();
     ULogger::setBuffered(true);
     EXPECT_TRUE(ULogger::isBuffered());
     
@@ -287,9 +293,8 @@ TEST(ULoggerTest, SetBuffered)
     EXPECT_FALSE(ULogger::isBuffered());
 }
 
-TEST(ULoggerTest, FileLogging)
+TEST_F(ULoggerTest, FileLogging)
 {
-    ULogger::reset();
     std::string testFile = "test_ulogger_file.txt";
     
     // Remove file if exists
@@ -313,9 +318,8 @@ TEST(ULoggerTest, FileLogging)
     EXPECT_GT(UFile::length(testFile), 0);
 }
 
-TEST(ULoggerTest, FileLoggingAppend)
+TEST_F(ULoggerTest, FileLoggingAppend)
 {
-    ULogger::reset();
     std::string testFile = "test_ulogger_append.txt";
     
     // Remove file if exists
@@ -343,9 +347,8 @@ TEST(ULoggerTest, FileLoggingAppend)
     EXPECT_GT(length2, length1);
 }
 
-TEST(ULoggerTest, BufferedLogging)
+TEST_F(ULoggerTest, BufferedLogging)
 {
-    ULogger::reset();
     std::string testFile = "test_ulogger_buffered.txt";
     
     // Remove file if exists
@@ -371,7 +374,7 @@ TEST(ULoggerTest, BufferedLogging)
     EXPECT_GT(UFile::length(testFile), 0);
 }
 
-TEST(ULoggerTest, GetTime)
+TEST_F(ULoggerTest, GetTime)
 {
     std::string timeStr;
     int result = ULogger::getTime(timeStr);
@@ -384,9 +387,8 @@ TEST(ULoggerTest, GetTime)
     EXPECT_NE(timeStr.find("-"), std::string::npos);
 }
 
-TEST(ULoggerTest, SetEventLevel)
+TEST_F(ULoggerTest, SetEventLevel)
 {
-    ULogger::reset();
 
     ULogger::setEventLevel(ULogger::kDebug);
     EXPECT_EQ(ULogger::eventLevel(), ULogger::kDebug);
@@ -401,9 +403,8 @@ TEST(ULoggerTest, SetEventLevel)
     EXPECT_EQ(ULogger::eventLevel(), ULogger::kError);
 }
 
-TEST(ULoggerTest, RegisterCurrentThread)
+TEST_F(ULoggerTest, RegisterCurrentThread)
 {
-    ULogger::reset();
     ULogger::registerCurrentThread("TestThread");
     
     std::map<std::string, unsigned long> threads = ULogger::getRegisteredThreads();
@@ -416,9 +417,8 @@ TEST(ULoggerTest, RegisterCurrentThread)
     EXPECT_EQ(threads.find("TestThread"), threads.end());
 }
 
-TEST(ULoggerTest, ThreadIdFilter)
+TEST_F(ULoggerTest, ThreadIdFilter)
 {
-    ULogger::reset();
 
     unsigned long currentId = UThread::currentThreadId();
     std::set<unsigned long> filter;
@@ -452,9 +452,8 @@ private:
     bool logOnce_;
 };
 
-TEST(ULoggerTest, ThreadIdFilterVector)
+TEST_F(ULoggerTest, ThreadIdFilterVector)
 {
-    ULogger::reset();
 
     // Both threads are registering to logger
     ULogger::registerCurrentThread("FilterThread");
@@ -513,9 +512,8 @@ TEST(ULoggerTest, ThreadIdFilterVector)
     ULogger::unregisterCurrentThread();
 }
 
-TEST(ULoggerTest, WriteWithLevel)
+TEST_F(ULoggerTest, WriteWithLevel)
 {
-    ULogger::reset();
 
     std::string testFile = "logger_write_level_test.txt";
     ULogger::setType(ULogger::kTypeFile, testFile, false);
@@ -550,9 +548,8 @@ TEST(ULoggerTest, WriteWithLevel)
     file.close(); 
 }
 
-TEST(ULoggerTest, Macros)
+TEST_F(ULoggerTest, Macros)
 {
-    ULogger::reset();
     std::string testFile = "logger_macros_test.txt";
     ULogger::setType(ULogger::kTypeFile, testFile, false);
 
@@ -585,9 +582,8 @@ TEST(ULoggerTest, Macros)
     file.close(); 
 }
 
-TEST(ULoggerTest, Reset)
+TEST_F(ULoggerTest, Reset)
 {
-    ULogger::reset();
     // Change some settings
     ULogger::setType(ULogger::kTypeConsole);
     ULogger::setLevel(ULogger::kDebug);
@@ -606,9 +602,8 @@ TEST(ULoggerTest, Reset)
     EXPECT_FALSE(ULogger::isBuffered());
 }
 
-TEST(ULoggerTest, MultipleLogLevels)
+TEST_F(ULoggerTest, MultipleLogLevels)
 {
-    ULogger::reset();
     std::string testFile = "logger_multiple_levels_test.txt";
     ULogger::setType(ULogger::kTypeFile, testFile, false);
     
@@ -656,9 +651,8 @@ TEST(ULoggerTest, MultipleLogLevels)
     file.close(); 
 }
 
-TEST(ULoggerTest, FileContent)
+TEST_F(ULoggerTest, FileContent)
 {
-    ULogger::reset();
     std::string testFile = "test_ulogger_content.txt";
     
     ULogger::setType(ULogger::kTypeFile, testFile, false);
@@ -679,9 +673,8 @@ TEST(ULoggerTest, FileContent)
     EXPECT_NE(content.find("Test content message"), std::string::npos);
 }
 
-TEST(ULoggerTest, ThreadSafety)
+TEST_F(ULoggerTest, ThreadSafety)
 {
-    ULogger::reset();
     std::string testFile = "logger_thread_safety_test.txt";
     ULogger::setType(ULogger::kTypeFile, testFile, false);
 
@@ -739,14 +732,13 @@ TEST(ULoggerTest, ThreadSafety)
     file.close(); 
 }
 
-TEST(ULoggerTest, DefaultLogFileName)
+TEST_F(ULoggerTest, DefaultLogFileName)
 {
     EXPECT_EQ(ULogger::kDefaultLogFileName, "./ULog.txt");
 }
 
-TEST(ULoggerTest, AllPrintOptions)
+TEST_F(ULoggerTest, AllPrintOptions)
 {
-    ULogger::reset();
     std::string testFile = "logger_all_print_options_test.txt";
     ULogger::setType(ULogger::kTypeFile, testFile, false);