Added IMUThread and IMUFilter tests

2026-10-07 18:47:48 +08:00 · 2026-05-17 14:20:34 -07:00
parent b0e0498e19
commit 7fdf3a13f2
5 changed files with 617 additions and 18 deletions
@@ -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)
@@ -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
@@ -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);
+}