Write test case for kalman filter

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

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