Add tests

source/armarx/navigation/human/test/human_test.cpp

@@ -40,11 +40,13 @@
 
 #include <armarx/navigation/Test.h>
 #include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/human/MovementDistance.h>
 #include <armarx/navigation/human/types.h>
 
 using armarx::navigation::core::Pose2D;
 using armarx::navigation::human::EuclideanDistance;
 using armarx::navigation::human::OrientationDistance;
+using armarx::navigation::human::MovementDistance;
 using armarx::navigation::human::Human;
 
 BOOST_AUTO_TEST_CASE(testEuclideanDistance1_standardDistance)
@@ -91,6 +93,50 @@ BOOST_AUTO_TEST_CASE(testOrientationDistance1_faceToFace)
     BOOST_CHECK(d < 0.001);
 }
 
+BOOST_AUTO_TEST_CASE(testOrientationDistance2_backToBack)
+{
+    const OrientationDistance distance = OrientationDistance(7, 0);
+    Pose2D pose1 = Pose2D::Identity();
+    pose1.translation() = Eigen::Vector2f(2, 2);
+    pose1.linear() = Eigen::Rotation2Df(-M_PI_2).toRotationMatrix();
+    const Human h1 = {.pose = pose1,
+                      .linearVelocity = Eigen::Vector2f(0.2, 0.1),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    Pose2D pose2 = Pose2D::Identity();
+    pose2.translation() = Eigen::Vector2f(2, 3);
+    pose2.linear() = Eigen::Rotation2Df(M_PI_2).toRotationMatrix();
+    const Human h2 = {.pose = pose2,
+                      .linearVelocity = Eigen::Vector2f(0.5, 0.8),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    double d = distance.computeDistance(h1, h2);
+    printf("Distance is %.2f\n", d);
+    BOOST_CHECK_CLOSE(d, 7, 0.001);
+}
+
+BOOST_AUTO_TEST_CASE(testMovementDistance1_roughlyMovingTogether)
+{
+    const MovementDistance distance = MovementDistance();
+    Pose2D pose1 = Pose2D::Identity();
+    pose1.translation() = Eigen::Vector2f(-2, 3);
+    pose1.linear() = Eigen::Rotation2Df(-M_PI_2).toRotationMatrix();
+    const Human h1 = {.pose = pose1,
+                      .linearVelocity = Eigen::Vector2f(1.0, 1.0),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    Pose2D pose2 = Pose2D::Identity();
+    pose2.translation() = Eigen::Vector2f(2, 5);
+    pose2.linear() = Eigen::Rotation2Df(M_PI_2).toRotationMatrix();
+    const Human h2 = {.pose = pose2,
+                      .linearVelocity = Eigen::Vector2f(1.3, 1.4),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    double d = distance.computeDistance(h1, h2);
+    printf("Distance is %.2f\n", d);
+    BOOST_CHECK_CLOSE(d, 0.5, 0.001);
+}
+
 //BOOST_AUTO_TEST_CASE(testPathLength)
 //{
 //    armarx::navigation::core::Path path{