Added IMUThread and IMUFilter tests

corelib/include/rtabmap/core/IMUFilter.h

@@ -34,47 +34,109 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
+/**
+ * @class IMUFilter
+ * @brief Fuses gyroscope and accelerometer samples into an orientation quaternion.
+ *
+ * Implementations integrate angular rates and correct drift using the measured
+ * gravity vector. The public @ref update() API takes timestamps and computes the
+ * time step between consecutive calls.
+ *
+ * Factory methods @ref create() return heap-allocated instances; the caller owns
+ * the pointer (e.g. @ref SensorCaptureThread, @ref IMUThread, @ref Camera).
+ *
+ * Output orientation from @ref getOrientation() is a unit quaternion
+ * `(qx, qy, qz, qw)` in the same convention as @ref IMU (body frame).
+ *
+ * Filter tuning parameters are read from @ref ParametersMap (ImuFilter/... keys);
+ * see @ref ComplementaryFilter and @ref MadgwickFilter (when RTAB-Map is built
+ * with Madgwick support).
+ *
+ * @see IMU
+ * @see SensorCaptureThread::enableIMUFiltering()
+ */
 class RTABMAP_CORE_EXPORT IMUFilter
 {
 public:
+	/**
+	 * @brief Orientation fusion algorithm.
+	 */
 	enum Type {
-		kMadgwick=0,
-		kComplementaryFilter=1};
-public:
+		kMadgwick = 0,              /**< Madgwick AHRS (attitude and heading reference system). RTAB-Map must be built with Madgwick support. */
+		kComplementaryFilter = 1};  /**< Complementary filter (always available). */
+
+	/**
+	 * @brief Creates a filter using type parsed from @p parameters.
+	 * @param parameters Optional ImuFilter/... tuning parameters.
+	 * @return New filter instance (caller owns the pointer).
+	 */
 	static IMUFilter * create(const ParametersMap & parameters = ParametersMap());
+
+	/**
+	 * @brief Creates a filter of the given @p type.
+	 * @param type Fusion algorithm; falls back to @ref kComplementaryFilter if
+	 *             @ref kMadgwick is requested but not compiled in.
+	 * @param parameters Optional ImuFilter/... tuning parameters.
+	 * @return New filter instance (caller owns the pointer).
+	 */
 	static IMUFilter * create(IMUFilter::Type type, const ParametersMap & parameters = ParametersMap());
 
-public:
-	virtual void parseParameters(const ParametersMap & parameters) {}
-	virtual ~IMUFilter(){}
+	virtual ~IMUFilter() {}
 
+	/**
+	 * @brief Re-reads filter parameters from @p parameters.
+	 * @param parameters ImuFilter/... keys (implementation-specific).
+	 */
+	virtual void parseParameters(const ParametersMap & parameters) {}
+
+	/**
+	 * @brief Integrates one IMU sample and updates the internal orientation estimate.
+	 * @param gx Gyroscope x angular rate (rad/s).
+	 * @param gy Gyroscope y angular rate (rad/s).
+	 * @param gz Gyroscope z angular rate (rad/s).
+	 * @param ax Accelerometer x (m/s²; magnitude ~9.81 when stationary).
+	 * @param ay Accelerometer y (m/s²).
+	 * @param az Accelerometer z (m/s²).
+	 * @param stamp Sample time (seconds); used with the previous stamp to compute `dt`.
+	 */
 	void update(
 			double gx, double gy, double gz,
 			double ax, double ay, double az,
 			double stamp);
 
+	/** @return Active fusion algorithm type. */
 	virtual IMUFilter::Type type() const = 0;
+
+	/**
+	 * @brief Current orientation estimate.
+	 * @param qx Quaternion x
+	 * @param qy Quaternion y
+	 * @param qz Quaternion z
+	 * @param qw Quaternion w (scalar)
+	 */
 	virtual void getOrientation(double & qx, double & qy, double & qz, double & qw) const = 0;
+
+	/**
+	 * @brief Resets internal state to the given orientation.
+	 * @param qx Quaternion x (default 0)
+	 * @param qy Quaternion y (default 0)
+	 * @param qz Quaternion z (default 0)
+	 * @param qw Quaternion w (default 1, identity)
+	 */
 	virtual void reset(double qx = 0.0, double qy = 0.0, double qz = 0.0, double qw = 1.0) = 0;
 
 protected:
 	IMUFilter(const ParametersMap & parameters = ParametersMap()) : previousStamp_(0) {}
 
 private:
-	// Update from accelerometer and gyroscope data.
-	// [gx, gy, gz]: Angular veloctiy, in rad / s.
-	// [ax, ay, az]: Normalized gravity vector.
-	// dt: time delta, in seconds.
 	virtual void updateImpl(
 			double gx, double gy, double gz,
 			double ax, double ay, double az,
 			double dt) = 0;
 
-private:
 	double previousStamp_;
 };
 
-}
-
+} // namespace rtabmap
 
 #endif /* CORELIB_INCLUDE_RTABMAP_CORE_IMUFILTER_H_ */

corelib/include/rtabmap/core/IMUThread.h

@@ -37,26 +37,63 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <fstream>
 
-namespace rtabmap
-{
+namespace rtabmap {
 
 class IMUFilter;
 
 /**
- * Class IMUThread
+ * @class IMUThread
+ * @brief Background thread that replays IMU measurements from a CSV file.
  *
+ * Reads gyroscope and accelerometer samples from a comma-separated file (EuRoC-style
+ * or epoch timestamps), optionally fuses them with @ref IMUFilter, and posts
+ * @ref IMUEvent on each step.
+ *
+ * CSV format:
+ * - First line: header (skipped).
+ * - Following lines: `stamp,gx,gy,gz,ax,ay,az` (stamp in seconds with a decimal
+ *   point, or EuRoC nanoseconds without one).
+ *
+ * Playback rate is limited by @ref setRate() when `rate > 0`; otherwise samples are
+ * read as fast as possible (or spaced using inter-sample timestamps after the first
+ * pair). When the file ends, the thread posts an empty @ref IMUEvent and stops.
+ *
+ * @see IMUEvent
+ * @see IMUFilter
+ * @see SensorCaptureThread::enableIMUFiltering()
  */
 class RTABMAP_CORE_EXPORT IMUThread :
 	public UThread,
 	public UEventsSender
 {
 public:
+	/**
+	 * @brief Constructs the replay thread.
+	 * @param rate Target playback rate in Hz (`0` = no rate cap until timestamps apply).
+	 * @param localTransform IMU frame relative to the robot base (stored in each @ref IMU).
+	 */
 	IMUThread(int rate, const Transform & localTransform);
 	virtual ~IMUThread();
 
+	/**
+	 * @brief Opens and validates an IMU CSV file.
+	 * @param path Path to the CSV file.
+	 * @return False if the file is missing or contains no data rows.
+	 */
 	bool init(const std::string & path);
+
+	/** @brief Sets the target playback rate in Hz. */
 	void setRate(int rate);
-	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
+
+	/**
+	 * @brief Enables orientation fusion on replayed samples.
+	 * @param filteringStrategy @ref IMUFilter::Type index (`0` = Madgwick, `1` = complementary).
+	 * @param parameters Optional ImuFilter/... tuning parameters.
+	 * @param baseFrameConversion If true, rotate IMU vectors into the base frame before filtering.
+	 */
+	void enableIMUFiltering(int filteringStrategy = 1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
+
+	/** @brief Disables fusion and deletes the internal @ref IMUFilter. */
 	void disableIMUFiltering();
 
 private:

corelib/test/CMakeLists.txt

@@ -111,6 +111,16 @@ add_executable(test_imu test_imu.cpp)
 target_link_libraries(test_imu gtest_main rtabmap_core)
 add_test(NAME test_imu COMMAND test_imu)
 
+#IMUFilter.h
+add_executable(test_imufilter test_imufilter.cpp)
+target_link_libraries(test_imufilter gtest_main rtabmap_core)
+add_test(NAME test_imufilter COMMAND test_imufilter)
+
+#IMUThread.h
+add_executable(test_imuthread test_imuthread.cpp)
+target_link_libraries(test_imuthread gtest_main rtabmap_core)
+add_test(NAME test_imuthread COMMAND test_imuthread)
+
 #Graph.h
 add_executable(test_graph test_graph.cpp)
 target_link_libraries(test_graph gtest_main rtabmap_core)

corelib/test/test_imufilter.cpp

@@ -0,0 +1,174 @@
+#include <gtest/gtest.h>
+#include <rtabmap/core/IMUFilter.h>
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <cmath>
+#include <memory>
+
+using namespace rtabmap;
+
+namespace {
+
+static constexpr double kGravity = 9.81;
+
+static void expectQuatNear(
+		double qx,
+		double qy,
+		double qz,
+		double qw,
+		double ex,
+		double ey,
+		double ez,
+		double ew,
+		double tol = 1e-3)
+{
+	EXPECT_NEAR(qx, ex, tol);
+	EXPECT_NEAR(qy, ey, tol);
+	EXPECT_NEAR(qz, ez, tol);
+	EXPECT_NEAR(qw, ew, tol);
+}
+
+static double quatNorm(double qx, double qy, double qz, double qw)
+{
+	return std::sqrt(qx * qx + qy * qy + qz * qz + qw * qw);
+}
+
+static ParametersMap complementaryParams(
+		double gainAcc = 0.2,
+		bool biasEstimation = false)
+{
+	ParametersMap params;
+	params.insert(ParametersPair(Parameters::kImuFilterComplementaryGainAcc(), uNumber2Str(gainAcc)));
+	params.insert(ParametersPair(
+			Parameters::kImuFilterComplementaryDoBiasEstimation(),
+			biasEstimation ? "true" : "false"));
+	params.insert(ParametersPair(Parameters::kImuFilterComplementaryDoAdpativeGain(), "false"));
+	return params;
+}
+
+static void feedStaticLevel(IMUFilter & filter, double stamp, int count = 20, double dt = 0.01)
+{
+	for(int i = 0; i < count; ++i)
+	{
+		filter.update(0, 0, 0, 0, 0, kGravity, stamp + i * dt);
+	}
+}
+
+static void feedYawRate(IMUFilter & filter, double gz, double stamp, int count = 50, double dt = 0.01)
+{
+	for(int i = 0; i < count; ++i)
+	{
+		filter.update(0, 0, gz, 0, 0, kGravity, stamp + i * dt);
+	}
+}
+
+} // namespace
+
+TEST(IMUFilterTest, CreateComplementaryFilter)
+{
+	std::unique_ptr<IMUFilter> filter(IMUFilter::create(IMUFilter::kComplementaryFilter));
+	ASSERT_NE(filter.get(), nullptr);
+	EXPECT_EQ(filter->type(), IMUFilter::kComplementaryFilter);
+}
+
+TEST(IMUFilterTest, CreateMadgwickOrFallback)
+{
+	std::unique_ptr<IMUFilter> filter(IMUFilter::create(IMUFilter::kMadgwick));
+	ASSERT_NE(filter.get(), nullptr);
+#ifdef RTABMAP_MADGWICK
+	EXPECT_EQ(filter->type(), IMUFilter::kMadgwick);
+#else
+	EXPECT_EQ(filter->type(), IMUFilter::kComplementaryFilter);
+#endif
+}
+
+TEST(IMUFilterTest, CreateFromParametersMap)
+{
+	std::unique_ptr<IMUFilter> filter(IMUFilter::create(complementaryParams()));
+	ASSERT_NE(filter.get(), nullptr);
+	EXPECT_EQ(filter->type(), IMUFilter::kComplementaryFilter);
+}
+
+TEST(IMUFilterTest, ResetSetsOrientation)
+{
+	std::unique_ptr<IMUFilter> filter(
+			IMUFilter::create(IMUFilter::kComplementaryFilter, complementaryParams()));
+
+	filter->reset();
+	double qx = 0, qy = 0, qz = 0, qw = 0;
+	filter->getOrientation(qx, qy, qz, qw);
+	expectQuatNear(qx, qy, qz, qw, 0, 0, 0, 1, 1e-4);
+
+	// Use a unit quaternion (reset stores its inverse internally).
+	const double qxIn = 0.1, qyIn = 0.2, qzIn = 0.3;
+	const double qwIn = std::sqrt(1.0 - qxIn * qxIn - qyIn * qyIn - qzIn * qzIn);
+	filter->reset(qxIn, qyIn, qzIn, qwIn);
+	filter->getOrientation(qx, qy, qz, qw);
+	expectQuatNear(qx, qy, qz, qw, qxIn, qyIn, qzIn, qwIn, 1e-4);
+	EXPECT_NEAR(quatNorm(qx, qy, qz, qw), 1.0, 1e-4);
+}
+
+TEST(IMUFilterTest, StaticAccelerometerNearIdentity)
+{
+	std::unique_ptr<IMUFilter> filter(
+			IMUFilter::create(IMUFilter::kComplementaryFilter, complementaryParams()));
+	feedStaticLevel(*filter, 0.0);
+
+	double qx = 0, qy = 0, qz = 0, qw = 0;
+	filter->getOrientation(qx, qy, qz, qw);
+	expectQuatNear(qx, qy, qz, qw, 0, 0, 0, 1, 0.05);
+	EXPECT_NEAR(quatNorm(qx, qy, qz, qw), 1.0, 1e-4);
+}
+
+TEST(IMUFilterTest, OrientationStaysNormalized)
+{
+	std::unique_ptr<IMUFilter> filter(
+			IMUFilter::create(IMUFilter::kComplementaryFilter, complementaryParams()));
+	feedStaticLevel(*filter, 0.0, 100);
+
+	double qx = 0, qy = 0, qz = 0, qw = 0;
+	filter->getOrientation(qx, qy, qz, qw);
+	EXPECT_NEAR(quatNorm(qx, qy, qz, qw), 1.0, 1e-4);
+}
+
+TEST(IMUFilterTest, GyroIntegrationChangesYaw)
+{
+	std::unique_ptr<IMUFilter> filter(
+			IMUFilter::create(IMUFilter::kComplementaryFilter, complementaryParams(0.01, false)));
+	feedStaticLevel(*filter, 0.0, 5);
+	feedYawRate(*filter, 1.0, 0.1, 80, 0.01);
+
+	double qx = 0, qy = 0, qz = 0, qw = 0;
+	filter->getOrientation(qx, qy, qz, qw);
+	EXPECT_GT(std::fabs(qz), 0.05);
+	EXPECT_NEAR(quatNorm(qx, qy, qz, qw), 1.0, 1e-3);
+}
+
+TEST(IMUFilterTest, ParseParametersAffectsFilter)
+{
+	ParametersMap params = complementaryParams(0.5, false);
+	std::unique_ptr<IMUFilter> filter(IMUFilter::create(IMUFilter::kComplementaryFilter, params));
+	filter->parseParameters(complementaryParams(0.01, false));
+	feedStaticLevel(*filter, 0.0);
+
+	double qx = 0, qy = 0, qz = 0, qw = 0;
+	filter->getOrientation(qx, qy, qz, qw);
+	EXPECT_NEAR(quatNorm(qx, qy, qz, qw), 1.0, 1e-4);
+}
+
+#ifdef RTABMAP_MADGWICK
+TEST(IMUFilterTest, MadgwickStaticAccelerometerNearIdentity)
+{
+	ParametersMap params;
+	params.insert(ParametersPair(Parameters::kImuFilterMadgwickGain(), "0.1"));
+	params.insert(ParametersPair(Parameters::kImuFilterMadgwickZeta(), "0.0"));
+
+	std::unique_ptr<IMUFilter> filter(IMUFilter::create(IMUFilter::kMadgwick, params));
+	feedStaticLevel(*filter, 0.0);
+
+	double qx = 0, qy = 0, qz = 0, qw = 0;
+	filter->getOrientation(qx, qy, qz, qw);
+	expectQuatNear(qx, qy, qz, qw, 0, 0, 0, 1, 0.1);
+	EXPECT_NEAR(quatNorm(qx, qy, qz, qw), 1.0, 1e-4);
+}
+#endif

corelib/test/test_imuthread.cpp

@@ -0,0 +1,316 @@
+#include <gtest/gtest.h>
+#include <rtabmap/core/IMUThread.h>
+#include <rtabmap/core/IMU.h>
+#include <rtabmap/core/IMUFilter.h>
+#include <rtabmap/utilite/UEventsHandler.h>
+#include <rtabmap/utilite/UEventsManager.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <fstream>
+#include <cmath>
+#include <unistd.h>
+#include <vector>
+
+using namespace rtabmap;
+
+namespace {
+
+static int g_fileCounter = 0;
+
+static std::string tempImuCsvPath()
+{
+	return uFormat("/tmp/rtabmap_imuthread_test_%d_%d.csv", getpid(), ++g_fileCounter);
+}
+
+static bool writeImuCsv(const std::string & path, const std::vector<std::string> & rows)
+{
+	std::ofstream file(path.c_str());
+	if(!file.good())
+	{
+		return false;
+	}
+	file << "#timestamp,wx,wy,wz,ax,ay,az\n";
+	for(size_t i = 0; i < rows.size(); ++i)
+	{
+		file << rows[i] << "\n";
+	}
+	return file.good();
+}
+
+static bool orientationSet(const cv::Vec4d & orientation)
+{
+	return orientation[0] != 0.0 || orientation[1] != 0.0 || orientation[2] != 0.0 || orientation[3] != 0.0;
+}
+
+static void expectVec3Near(const cv::Vec3d & a, const cv::Vec3d & b, double tol = 1e-5)
+{
+	EXPECT_NEAR(a[0], b[0], tol);
+	EXPECT_NEAR(a[1], b[1], tol);
+	EXPECT_NEAR(a[2], b[2], tol);
+}
+
+static void expectQuatNear(
+		const cv::Vec4d & q,
+		double ex,
+		double ey,
+		double ez,
+		double ew,
+		double tol = 1e-3)
+{
+	EXPECT_NEAR(q[0], ex, tol);
+	EXPECT_NEAR(q[1], ey, tol);
+	EXPECT_NEAR(q[2], ez, tol);
+	EXPECT_NEAR(q[3], ew, tol);
+}
+
+class IMUEventCollector : public UEventsHandler
+{
+public:
+	struct Sample
+	{
+		IMU data;
+		double stamp;
+		bool valid;
+	};
+
+	void clear() { samples_.clear(); }
+	const std::vector<Sample> & samples() const { return samples_; }
+
+protected:
+	virtual bool handleEvent(UEvent * event)
+	{
+		if(event->getClassName() == "IMUEvent")
+		{
+			const IMUEvent * imuEvent = static_cast<IMUEvent *>(event);
+			Sample sample;
+			sample.data = imuEvent->getData();
+			sample.stamp = imuEvent->getStamp();
+			sample.valid = !imuEvent->getData().empty();
+			samples_.push_back(sample);
+		}
+		return false;
+	}
+
+private:
+	std::vector<Sample> samples_;
+};
+
+static std::vector<IMUEventCollector::Sample> runThread(
+		IMUThread & thread,
+		size_t minValidSamples = 0,
+		double maxWaitSec = 2.0)
+{
+	IMUEventCollector collector;
+	UEventsManager::addHandler(&collector);
+	thread.start();
+
+	UTimer timer;
+	while(timer.ticks() < maxWaitSec)
+	{
+		if(thread.isKilled())
+		{
+			break;
+		}
+		if(minValidSamples > 0)
+		{
+			size_t validCount = 0;
+			for(size_t i = 0; i < collector.samples().size(); ++i)
+			{
+				if(collector.samples()[i].valid)
+				{
+					++validCount;
+				}
+			}
+			if(validCount >= minValidSamples)
+			{
+				break;
+			}
+		}
+		uSleep(5);
+	}
+
+	if(!thread.isKilled())
+	{
+		thread.kill();
+	}
+	thread.join(true);
+	UEventsManager::removeHandler(&collector);
+	return collector.samples();
+}
+
+} // namespace
+
+TEST(IMUThreadTest, InitFailsOnMissingFile)
+{
+	IMUThread thread(0, Transform::getIdentity());
+	EXPECT_FALSE(thread.init("/tmp/rtabmap_imuthread_missing_file.csv"));
+}
+
+TEST(IMUThreadTest, InitFailsOnHeaderOnly)
+{
+	const std::string path = tempImuCsvPath();
+	ASSERT_TRUE(writeImuCsv(path, std::vector<std::string>()));
+
+	IMUThread thread(0, Transform::getIdentity());
+	EXPECT_FALSE(thread.init(path));
+	UFile::erase(path);
+}
+
+TEST(IMUThreadTest, InitSucceedsWithValidFile)
+{
+	const std::string path = tempImuCsvPath();
+	ASSERT_TRUE(writeImuCsv(path, {"1.0,0,0,0,0,0,9.81"}));
+
+	IMUThread thread(0, Transform::getIdentity());
+	EXPECT_TRUE(thread.init(path));
+	UFile::erase(path);
+}
+
+TEST(IMUThreadTest, PublishesSamplesFromCsv)
+{
+	const std::string path = tempImuCsvPath();
+	// Equal stamps avoid captureDelay busy-wait when rate is 0.
+	ASSERT_TRUE(writeImuCsv(path, {
+			"1.0,0.1,0.2,0.3,0.0,0.0,9.81",
+			"1.0,0.2,0.3,0.4,0.0,0.0,9.81"}));
+
+	IMUThread thread(0, Transform::getIdentity());
+	ASSERT_TRUE(thread.init(path));
+
+	const std::vector<IMUEventCollector::Sample> samples = runThread(thread, 2);
+	ASSERT_GE(samples.size(), 2u);
+
+	EXPECT_TRUE(samples[0].valid);
+	EXPECT_NEAR(samples[0].stamp, 1.0, 1e-6);
+	expectVec3Near(samples[0].data.angularVelocity(), cv::Vec3d(0.1, 0.2, 0.3));
+	expectVec3Near(samples[0].data.linearAcceleration(), cv::Vec3d(0.0, 0.0, 9.81));
+
+	EXPECT_TRUE(samples[1].valid);
+	EXPECT_NEAR(samples[1].stamp, 1.0, 1e-6);
+
+	// End-of-file posts an invalid/empty event then kills the thread.
+	EXPECT_FALSE(samples.back().valid);
+
+	UFile::erase(path);
+}
+
+TEST(IMUThreadTest, PublishesEurocStampsInSeconds)
+{
+	// EuRoC IMU CSV: integer timestamp = seconds * 1e9 + nanoseconds (no '.').
+	// 10.5 s  -> "10500000000",  10.6 s -> "10600000000"
+	const std::string path = tempImuCsvPath();
+	ASSERT_TRUE(writeImuCsv(path, {
+			"10500000000,0.1,0.2,0.3,0.0,0.0,9.81",
+			"10600000000,0.2,0.3,0.4,0.0,0.0,9.81"}));
+
+	IMUThread thread(0, Transform::getIdentity());
+	ASSERT_TRUE(thread.init(path));
+
+	const std::vector<IMUEventCollector::Sample> samples = runThread(thread, 2);
+	ASSERT_GE(samples.size(), 2u);
+
+	EXPECT_TRUE(samples[0].valid);
+	EXPECT_NEAR(samples[0].stamp, 10.5, 1e-9);
+	EXPECT_TRUE(samples[1].valid);
+	EXPECT_NEAR(samples[1].stamp, 10.6, 1e-9);
+
+	UFile::erase(path);
+}
+
+TEST(IMUThreadTest, EnableFilteringSetsOrientation)
+{
+	const std::string path = tempImuCsvPath();
+	ASSERT_TRUE(writeImuCsv(path, {
+			"0.0,0,0,0,0,0,9.81",
+			"0.01,0,0,0,0,0,9.81",
+			"0.02,0,0,0,0,0,9.81",
+			"0.03,0,0,0,0,0,9.81",
+			"0.04,0,0,0,0,0,9.81"}));
+
+	IMUThread thread(0, Transform::getIdentity());
+	ASSERT_TRUE(thread.init(path));
+	thread.enableIMUFiltering(IMUFilter::kComplementaryFilter);
+
+	const std::vector<IMUEventCollector::Sample> samples = runThread(thread, 3);
+	ASSERT_FALSE(samples.empty());
+
+	bool foundOrientation = false;
+	for(int i = static_cast<int>(samples.size()) - 1; i >= 0; --i)
+	{
+		if(samples[i].valid && orientationSet(samples[i].data.orientation()))
+		{
+			foundOrientation = true;
+			const cv::Vec4d & q = samples[i].data.orientation();
+			const double norm = std::sqrt(
+					q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
+			EXPECT_NEAR(norm, 1.0, 0.05);
+			// Static gravity, zero gyro: filter should stay near identity.
+			expectQuatNear(q, 0, 0, 0, 1, 0.05);
+			break;
+		}
+	}
+	EXPECT_TRUE(foundOrientation);
+
+	UFile::erase(path);
+}
+
+TEST(IMUThreadTest, DisableFilteringLeavesOrientationUnset)
+{
+	const std::string path = tempImuCsvPath();
+	ASSERT_TRUE(writeImuCsv(path, {"1.0,0.1,0.2,0.3,0.0,0.0,9.81"}));
+
+	IMUThread thread(0, Transform::getIdentity());
+	ASSERT_TRUE(thread.init(path));
+	thread.enableIMUFiltering(IMUFilter::kComplementaryFilter);
+	thread.disableIMUFiltering();
+
+	const std::vector<IMUEventCollector::Sample> samples = runThread(thread, 1);
+	ASSERT_GE(samples.size(), 1u);
+	EXPECT_TRUE(samples[0].valid);
+	EXPECT_FALSE(orientationSet(samples[0].data.orientation()));
+
+	UFile::erase(path);
+}
+
+TEST(IMUThreadTest, StoresLocalTransformWithoutConvertingAcceleration)
+{
+	// Without IMU filtering, localTransform is only attached; acc/gyro are not rotated.
+	const std::string path = tempImuCsvPath();
+	ASSERT_TRUE(writeImuCsv(path, {"1.0,0,0,0,0,0,9.81"}));
+
+	const Transform local(0.1f, 0.2f, 0.3f, 0.f, 0.f, 0.5f);
+	IMUThread thread(0, local);
+	ASSERT_TRUE(thread.init(path));
+
+	const std::vector<IMUEventCollector::Sample> samples = runThread(thread, 1);
+	ASSERT_GE(samples.size(), 1u);
+	EXPECT_TRUE(samples[0].valid);
+	expectVec3Near(samples[0].data.linearAcceleration(), cv::Vec3d(0, 0, 9.81));
+	EXPECT_FLOAT_EQ(samples[0].data.localTransform().x(), 0.1f);
+	EXPECT_FLOAT_EQ(samples[0].data.localTransform().theta(), 0.5f);
+
+	UFile::erase(path);
+}
+
+TEST(IMUThreadTest, BaseFrameConversionRotatesAcceleration)
+{
+	// With filtering and baseFrameConversion=true, IMUThread calls convertToBaseFrame()
+	// before fusion (same rotation as IMU::convertToBaseFrame()).
+	const std::string path = tempImuCsvPath();
+	ASSERT_TRUE(writeImuCsv(path, {"1.0,0,0,0,1.0,0.0,0.0"}));
+
+	const float halfPi = static_cast<float>(CV_PI / 2.0);
+	const Transform local(0.f, 0.f, 0.f, 0.f, 0.f, halfPi);
+	IMUThread thread(0, local);
+	ASSERT_TRUE(thread.init(path));
+	thread.enableIMUFiltering(IMUFilter::kComplementaryFilter, ParametersMap(), true);
+
+	const std::vector<IMUEventCollector::Sample> samples = runThread(thread, 1);
+	ASSERT_GE(samples.size(), 1u);
+	EXPECT_TRUE(samples[0].valid);
+	expectVec3Near(samples[0].data.linearAcceleration(), cv::Vec3d(0, 1, 0), 1e-4);
+	EXPECT_NEAR(samples[0].data.localTransform().theta(), 0.0f, 1e-5f);
+
+	UFile::erase(path);
+}