From 9b8d0be53ba934a8c7928ee3d0116488ad532e4c Mon Sep 17 00:00:00 2001
From: Corvin-N <corvin@navarro.de>
Date: Thu, 22 Sep 2022 19:31:14 +0200
Subject: [PATCH] Write test case for kalman filter
---
source/armarx/navigation/human/CMakeLists.txt | 2 +-
.../human/test/kalman_filter_test.cpp | 92 +++++++++++++++++++
2 files changed, 93 insertions(+), 1 deletion(-)
diff --git a/source/armarx/navigation/human/CMakeLists.txt b/source/armarx/navigation/human/CMakeLists.txt
index 7ecd7127..4cf568f3 100644
--- a/source/armarx/navigation/human/CMakeLists.txt
+++ b/source/armarx/navigation/human/CMakeLists.txt
@@ -86,7 +86,7 @@ armarx_add_test(se3_kalman_test
)
-armarx_add_test(human_test
+armarx_add_test(kalman_filter_test
TEST_FILES
test/kalman_filter_test.cpp
DEPENDENCIES
diff --git a/source/armarx/navigation/human/test/kalman_filter_test.cpp b/source/armarx/navigation/human/test/kalman_filter_test.cpp
index 885374fe..04cf7c69 100644
--- a/source/armarx/navigation/human/test/kalman_filter_test.cpp
+++ b/source/armarx/navigation/human/test/kalman_filter_test.cpp
@@ -18,3 +18,95 @@
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
+
+#include <ArmarXCore/core/time/DateTime.h>
+
+#include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/human/HumanFilter.h>
+#include <armarx/navigation/human/types.h>
+
+// test includes
+#define BOOST_TEST_MODULE Navigation::ArmarXLibraries::human
+#define ARMARX_BOOST_TEST
+
+#include <armarx/navigation/Test.h>
+
+using armarx::navigation::core::Pose2D;
+using Eigen::Vector2f;
+
+namespace armarx::navigation::human
+{
+ Pose2D
+ generatePose(double x, double y, double yaw)
+ {
+ Pose2D pose = Pose2D::Identity();
+ pose.translation() = Eigen::Vector2f(x, y);
+ pose.linear() = Eigen::Rotation2Df(yaw).toRotationMatrix();
+
+ return pose;
+ }
+
+ DateTime
+ generateTime(double time)
+ {
+ return DateTime(time);
+ }
+
+ double
+ extractRotation(const Pose2D& pose)
+ {
+ return Eigen::Rotation2Df(pose.linear()).angle();
+ }
+
+
+ bool
+ doublesEqual(const double d1, const double d2, const double relErr)
+ {
+ if (d1 == 0)
+ {
+ return d2 == 0;
+ }
+ return std::abs(d1 - d2) / d1 <= relErr;
+ }
+
+ bool
+ posesEqual(const Pose2D& p1, const Pose2D& p2, const double relErr)
+ {
+ return doublesEqual(p1.translation().x(), p2.translation().x(), relErr) &&
+ doublesEqual(p1.translation().y(), p2.translation().y(), relErr) &&
+ doublesEqual(extractRotation(p1), extractRotation(p2), relErr);
+ }
+
+ bool
+ vectorsEqual(const Vector2f& v1, const Vector2f v2, const double relErr)
+ {
+ return doublesEqual(v1.x(), v2.x(), relErr) && doublesEqual(v1.y(), v2.y(), relErr);
+ }
+
+ bool
+ timesEqual(const DateTime& t1, const DateTime& t2, const double relErr)
+ {
+ return doublesEqual(t1.toMilliSecondsSinceEpoch(), t2.toMilliSecondsSinceEpoch(), relErr);
+ }
+
+ bool
+ humansEqual(const Human& h1, const Human& h2, const double relErr)
+ {
+ return posesEqual(h1.pose, h2.pose, relErr) &&
+ vectorsEqual(h1.linearVelocity, h2.linearVelocity, relErr) &&
+ timesEqual(h1.detectionTime, h2.detectionTime, relErr);
+ }
+
+
+ BOOST_AUTO_TEST_CASE(testInitialization)
+ {
+ HumanFilter filter(generatePose(3, 2, 1), generateTime(4));
+
+ Human human{.pose = generatePose(3, 2, 1),
+ .linearVelocity = Vector2f::Zero(),
+ .detectionTime = generateTime(4)};
+
+ BOOST_TEST(humansEqual(filter.get(), human, 0.001),
+ "filter should not have changed humans initial pose properties");
+ }
+} // namespace armarx::navigation::human
--
GitLab