Draft: Implement human grouping

Closes #34

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

@@ -38,6 +38,7 @@
 
 #include <armarx/navigation/Test.h>
 #include <armarx/navigation/core/basic_types.h>
+#include <armarx/navigation/human/Test.h>
 #include <armarx/navigation/human/types.h>
 
 using armarx::navigation::core::Pose2D;
@@ -46,21 +47,24 @@ using armarx::navigation::human::Human;
 
 BOOST_AUTO_TEST_CASE(testEuclideanDistance)
 {
-    //    EuclideanDistance distance = EuclideanDistance();
-
-    //    Pose2D pose1 = Pose2D::Identity();
-    //    pose1.translation() = Eigen::Vector2f(3, 4);
-    //    pose1.linear() = Eigen::Rotation2Df(1.1).toRotationMatrix();
-    //    Human h1 = {.pose = pose1,
-    //                .linearVelocity = Eigen::Vector2f(0.2, 0.1),
-    //                .detectionTime = armarx::DateTime::Now()};
-
-    //    Pose2D pose2 = Pose2D::Identity();
-    //    pose2.translation() = Eigen::Vector2f(6, 8);
-    //    pose2.linear() = Eigen::Rotation2Df(2.3).toRotationMatrix();
-    //    Human h2 = {.pose = pose2,
-    //                .linearVelocity = Eigen::Vector2f(0.5, 0.8),
-    //                .detectionTime = armarx::DateTime::Now()};
+    const EuclideanDistance distance = EuclideanDistance();
+    Pose2D pose1 = Pose2D::Identity();
+    pose1.translation() = Eigen::Vector2f(3, 4);
+    pose1.linear() = Eigen::Rotation2Df(1.1).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(6, 8);
+    pose2.linear() = Eigen::Rotation2Df(2.3).toRotationMatrix();
+    const Human h2 = {.pose = pose2,
+                      .linearVelocity = Eigen::Vector2f(0.5, 0.8),
+                      .detectionTime = armarx::DateTime::Now()};
+
+    double d = distance.computeDistance(h1, h2);
+    ARMARX_INFO << "Distance is" + std::to_string(d);
+    BOOST_CHECK_CLOSE(d, 4, 0.001);
 }
 
 //BOOST_AUTO_TEST_CASE(testPathLength)